Beispiel #1
0
/*! Check if \p a lies beside \p b. */
template<class T, std::size_t N> inline pure bool beside(const cring<T>& a,
                                                          const vec<T,N>& b){
    return ((dist(a.c(), b) <  a.ri()) or
            (dist(a.c(), b) >= a.ro()));
}
dFloat64 dBezierSpline::FindClosestKnot (dBigVector& closestPoint, const dBigVector& point, int subdivitionSteps) const
{
	int startSpan = m_degree;
	dFloat64 bestU = 0.0f;
	dFloat64 distance2 = 1.0e10f;
	dBigVector closestControlPoint (m_controlPoints[0]);
	subdivitionSteps = dMax (subdivitionSteps, 1);
	dFloat64 scale = 1.0f / subdivitionSteps;
	for (int span = m_degree; span < (m_knotsCount - m_degree - 1); span ++) {
		dFloat64 param = 0.0f;
		for (int i = 0; i < subdivitionSteps; i ++) {
			dFloat64 u = m_knotVector[span] + (m_knotVector[span + 1] - m_knotVector[span]) * param;
			param += scale;
			dBigVector p (CurvePoint (u, span));
			dBigVector dp (p - point);
			dFloat64 dist2 = dp.DotProduct3(dp);
			if (dist2 < distance2) {
				bestU = u;
				startSpan = span;
				distance2 = dist2;
				closestControlPoint = p;
			}
		}
	}

	dBigVector derivatives[32];
	dFloat64 u0 = bestU;

	bool stop = false;
	for (int i = 0; (i < 20) && !stop; i ++) {
		CurveAllDerivatives (u0, derivatives);

		dBigVector dist (closestControlPoint - point);
		dFloat64 num = derivatives[1].DotProduct3(dist);
		dFloat64 den = derivatives[2].DotProduct3(dist) + derivatives[1].DotProduct3(derivatives[1]);
//		if (dAbs (den) < 1.0e-6f)
//			__debugbreak();
		
		dFloat64 u1 = dClamp(u0 - num / den, dFloat64(0.0), dFloat64 (1.0));
		if (u1 < m_knotVector[startSpan]) {
			startSpan --;
			dAssert (startSpan >= 0);
		} else if (u1 >= m_knotVector[startSpan + 1]) {
			startSpan ++;
			dAssert (startSpan < (m_knotsCount - m_degree));
		} 

		dAssert (startSpan >= m_degree);
		dAssert (startSpan <= (m_knotsCount - m_degree - 1));
		closestControlPoint = CurvePoint (u1, startSpan);
		//dFloat64 xxx0 = num * num;
		//dFloat64 xxx1 = dist % dist;
		//dFloat64 xxx2 = derivatives[1] % derivatives[1];
		//dFloat64 xxx3 = xxx1 * xxx2 * 1.0e-10;

		stop |= (dAbs (u1 - u0) < 1.0e-10) || (num * num < ((dist.DotProduct3(dist)) * (derivatives[1].DotProduct3(derivatives[1])) * 1.0e-10));
		u0 = u1;
	}

	closestPoint = closestControlPoint;
	return u0;
}
Beispiel #3
0
double CosineTree::MonteCarloError(CosineTree* node,
                                   CosineNodeQueue& treeQueue,
                                   arma::vec* addBasisVector1,
                                   arma::vec* addBasisVector2)
{
  std::vector<size_t> sampledIndices;
  arma::vec probabilities;
  
  // Sample O(log m) points from the input node's distribution.
  // 'm' is the number of columns present in the node.
  size_t numSamples = log(node->NumColumns()) + 1;  
  node->ColumnSamplesLS(sampledIndices, probabilities, numSamples);
  
  // Get pointer to the original dataset.
  arma::mat dataset = node->GetDataset();
  
  // Initialize weighted projection magnitudes as zeros.
  arma::vec weightedMagnitudes;
  weightedMagnitudes.zeros(numSamples);
  
  // Set size of projection vector, depending on whether additional basis
  // vectors are passed.
  size_t projectionSize;
  if(addBasisVector1 && addBasisVector2)
    projectionSize = treeQueue.size() + 2;
  else
    projectionSize = treeQueue.size();
  
  // For each sample, calculate the weighted projection onto the current basis.
  for(size_t i = 0; i < numSamples; i++)
  {
    // Initialize projection as a vector of zeros.
    arma::vec projection;
    projection.zeros(projectionSize);

    CosineTree *currentNode;
    CosineNodeQueue::const_iterator j = treeQueue.begin();
  
    size_t k = 0;
    // Compute the projection of the sampled vector onto the existing subspace.
    for(; j != treeQueue.end(); j++, k++)
    {
      currentNode = *j;
    
      projection(k) = arma::dot(dataset.col(sampledIndices[i]),
                                currentNode->BasisVector());
    }
    // If two additional vectors are passed, take their projections.
    if(addBasisVector1 && addBasisVector2)
    {
      projection(k++) = arma::dot(dataset.col(sampledIndices[i]),
                                  *addBasisVector1);
      projection(k) = arma::dot(dataset.col(sampledIndices[i]),
                                *addBasisVector2);
    }
    
    // Calculate the Frobenius norm squared of the projected vector.
    double frobProjection = arma::norm(projection, "frob");
    double frobProjectionSquared = frobProjection * frobProjection;
    
    // Calculate the weighted projection magnitude.
    weightedMagnitudes(i) = frobProjectionSquared / probabilities(i);
  }
  
  // Compute mean and standard deviation of the weighted samples.
  double mu = arma::mean(weightedMagnitudes);
  double sigma = arma::stddev(weightedMagnitudes);
  
  if(!sigma)
  {
    node->L2Error(node->FrobNormSquared() - mu);
    return (node->FrobNormSquared() - mu);
  }
  
  // Fit a normal distribution using the calculated statistics, and calculate a
  // lower bound on the magnitudes for the passed 'delta' parameter.
  boost::math::normal dist(mu, sigma);
  double lowerBound = boost::math::quantile(dist, delta);
  
  // Upper bound on the subspace reconstruction error.
  node->L2Error(node->FrobNormSquared() - lowerBound);
  
  return (node->FrobNormSquared() - lowerBound);
}
Beispiel #4
0
int main()
{
    std::default_random_engine generator;
    std::uniform_int_distribution<long> distribution(0,400);
    std::vector<std::vector<long>> vec(5000);
    
    // simulate large data
    std::for_each(vec.begin(),vec.end(),[&gen=generator,&dist=distribution](std::vector<long>& vec)
                 {
                   vec.resize(100);
                   std::generate(vec.begin(),vec.end(),[&gen=gen,&dist=dist](){ return dist(gen);});   
          
                 });
                
                
//    extend(vec);    // perform calculation with the large vector<vector<long>> decomposed into a single vector<long> 
    
//    no_extend(vec);  // perform the calculation with the large vector<vector<long>> as is
    
      single_threaded(vec); // single threaded computation
   
    
}
Beispiel #5
0
bool in_range(int y0,int x0,int y1,int x1,int r) {
	return dist(y0,x0,y1,x1)<=pow(r,2);
}
Beispiel #6
0
// resolves collisions between bodies, returning new body count
int collide(int n, body bodies[]) {
  // initialize disjoint set and bodies to include
  set* bsets[n];
  int include[n];
  for (int i = 0; i < n; i++) {
    bsets[i] = make_set(i);
    include[i] = 1;
  }

  // find largest object
  double maxrad = RADIUS(bodies[0].m);
  for (int i = 0; i < n; i++) {
    double rad = RADIUS(bodies[i].m);
    if (rad > maxrad)
      maxrad = rad;
  }

  // form mesh for collision detection
  mesh* m = mesh_new(maxrad * 2);
  for (int i = 0; i < n; i++)
    mesh_put(m, bodies[i].pos, i);

  // find collisions
  for (int i = 0; i < n; i++) {
    vector ipos = bodies[i].pos;
    double irad = RADIUS(bodies[i].m);

    // which bodies are in contact with this one?
    // look up position in mesh
    body_list* next = mesh_get(m, ipos, 1);
    for (body_list* cur = next; cur; cur = next) {
      // get candidate collider
      int j = cur->index;
      vector jpos = bodies[j].pos;
      double jrad = RADIUS(bodies[j].m);

      // merge sets of colliding objects
      if (dist(ipos, jpos) < (irad + jrad) * (irad + jrad))
        merge(bsets[i], bsets[j]);

      // traverse and free
      next = cur->next;
      free(cur);
    }
  }

  // free the mesh
  mesh_free(m);

  // merge objects
  for (int i = 0; i < n; i++) {
    int rootidx = get_value(find(bsets[i]));
    if (rootidx != i) {
      include[i] = 0;
      bodies[rootidx] = body_merge(bodies[rootidx], bodies[i]);
    }
  }

  // free sets
  for (int i = 0; i < n; i++)
    free(bsets[i]);

  // copy down
  int j = 0;
  for (int i = 0; i < n; i++) {
    if (include[i])
      bodies[j++] = bodies[i];
  }

  return j;
}
Beispiel #7
0
void Surf::BuildDistMap()
{
	int i, j;
	int nump = 101;

	//==== Load Point Vec ====//
	vector< vector< vec3d > > pvec;
	pvec.resize( nump );
	for ( i = 0 ; i < nump ; i++ )
	{
		pvec[i].resize( nump );
		double u = (double)i/(double)(nump-1);
		for ( j = 0 ; j < nump ; j++ )
		{
			double w = (double)j/(double)(nump-1);
			pvec[i][j] = CompPnt01( u, w );
		}
	}

	//==== Find U Dists ====//
	double maxUDist = 0.0;
	vector< double > uDistVec;
	for ( i = 0 ; i < nump ; i++ )
	{
		double sum_d = 0.0;
		for ( j = 1 ; j < nump ; j++  )
		{
			sum_d += dist( pvec[j-1][i], pvec[j][i] );
		}
		uDistVec.push_back( sum_d );

		if ( sum_d > maxUDist )
			maxUDist = sum_d;
	}
	if ( maxUDist < DBL_EPSILON )
		maxUDist = 0.000000001;

	//==== Find W Dists ====//
	double maxWDist = 0.0;
	vector< double > wDistVec;
	for ( i = 0 ; i < nump ; i++ )
	{
		double sum_d = 0.0;
		for ( j = 1 ; j < nump ; j++  )
		{
			sum_d += dist( pvec[i][j-1], pvec[i][j] );
		}
		wDistVec.push_back( sum_d );

		if ( sum_d > maxWDist )
			maxWDist = sum_d;
	}

	if ( maxWDist < DBL_EPSILON )
		maxWDist = 0.000000001;


	//==== Scale U Dists ====//
	double wu_ratio = m_MaxW/m_MaxU;
	m_UScaleMap.resize( uDistVec.size() );
	for ( i = 0 ; i < (int)uDistVec.size() ; i++ )
	{
		m_UScaleMap[i] = wu_ratio*(uDistVec[i]/maxWDist);

		if ( m_UScaleMap[i] < 0.00001 )
			m_UScaleMap[i] = 0.00001;
	}

	//==== Scale W Dists ====//
	double uw_ratio = m_MaxU/m_MaxW;
	m_WScaleMap.resize( wDistVec.size() );
	for ( i = 0 ; i < (int)wDistVec.size() ; i++ )
	{
		m_WScaleMap[i] = uw_ratio*(wDistVec[i]/maxUDist);

		if ( m_WScaleMap[i] < 0.00001 )
			m_WScaleMap[i] = 0.00001;
	}

	//==== Figure Out Which to Scale ====//
	double min_u_scale = 1.0e12;
	double max_u_scale = 0.0;
	for ( i = 0 ; i < (int)m_UScaleMap.size() ; i++ )
	{	
		if ( m_UScaleMap[i] < min_u_scale ) min_u_scale = m_UScaleMap[i];
		if ( m_UScaleMap[i] > max_u_scale ) max_u_scale = m_UScaleMap[i];
	}
	double u_ratio = max_u_scale/min_u_scale;

	double min_w_scale = 1.0e12;
	double max_w_scale = 0.0;
	for ( i = 0 ; i < (int)m_WScaleMap.size() ; i++ )
	{	
		if ( m_WScaleMap[i] < min_w_scale ) min_w_scale = m_WScaleMap[i];
		if ( m_WScaleMap[i] > max_w_scale ) max_w_scale = m_WScaleMap[i];
	}
	double w_ratio = max_w_scale/min_w_scale;

	if ( u_ratio > w_ratio )
		m_ScaleUFlag = true;
	else
		m_ScaleUFlag = false;
	


//char str[256];
//static int cnt = 0;
//sprintf( str, "uwscale_%d.dat", cnt );
//cnt++;
//	FILE* fp = fopen(str, "w");
//	fprintf( fp, "ws 1 2\n" );
//	fprintf( fp, "color green\n" );
//	for ( i = 0 ; i < (int)m_WScaleMap.size() ; i++ )
//	{
//		double u = (double)i/(double)(m_WScaleMap.size()-1);
//		fprintf( fp, "%f %f \n", u, m_WScaleMap[i]  );
//	}
//	fprintf( fp, "color blue\n" );
//	for ( i = 0 ; i < 1001 ; i++ )
//	{
//		double u = (double)i/(double)(1000);
//		fprintf( fp, "%f %f \n", u, GetWScale(u)  );
//	}
//	fclose( fp );

}
float dist ( const GsColor &c1, const GsColor &c2 )
 { 
   return dist ( GsPnt(c1.r,c1.g,c1.b), GsPnt(c2.r,c2.g,c2.b) );
 }
Beispiel #9
0
int main(void) {
	int i, j, k;
	
	while(scanf("%d %d %d %d %d", &inicio.x, &inicio.y, &fim.x, &fim.y, &num_obstaculos)) {
		if(inicio.x == 0 && inicio.y == 0 && fim.x == 0 && fim.y == 0 && num_obstaculos == 0) break;

		/* init */
		n = num_obstaculos*2 + 2;
		for(i = 0; i < n; i++)
			for(j = 0; j < n; j++)
				w[i][j] = -1;
		
		/* input */
		for(i = 0; i < num_obstaculos; i++)
			scanf("%d %d %d %d", &obsa[i].x, &obsa[i].y, &obsb[i].x, &obsb[i].y);

		/* processa/modela o grafo */
		/* adjacencia inicio-fim */
		w[INICIO][FIM] = w[FIM][INICIO] = dist(inicio, fim);
		for(i = 0; i < num_obstaculos; i++) {
			if(segments_intersect(inicio, fim, obsa[i], obsb[i])) {
				w[INICIO][FIM] = w[FIM][INICIO] = -1;
				break;
			}
		}
		/* adjacencias inicio-obs */
		for(i = 0; i < num_obstaculos; i++) {
			/* obsa */
			w[INICIO][PONTOA(i)] = w[PONTOA(i)][INICIO] = dist(inicio, obsa[i]);
			for(j = 0; j < num_obstaculos; j++) {
				if(j != i && segments_intersect(inicio, obsa[i], obsa[j], obsb[j])) {
					w[INICIO][PONTOA(i)] = w[PONTOA(i)][INICIO] = -1;
					break;
				}
			}
			/* obsb */
			w[INICIO][PONTOB(i)] = w[PONTOB(i)][INICIO] = dist(inicio, obsb[i]);
			for(j = 0; j < num_obstaculos; j++) {
				if(j != i && segments_intersect(inicio, obsb[i], obsa[j], obsb[j])) {
					w[INICIO][PONTOB(i)] = w[PONTOB(i)][INICIO] = -1;
					break;
				}
			}
		}
		/* adjacencias obs-fim */
		for(i = 0; i < num_obstaculos; i++) {
			/* obsa */
			w[FIM][PONTOA(i)] = w[PONTOA(i)][FIM] = dist(fim, obsa[i]);
			for(j = 0; j < num_obstaculos; j++) {
				if(j != i && segments_intersect(fim, obsa[i], obsa[j], obsb[j])) {
					w[FIM][PONTOA(i)] = w[PONTOA(i)][FIM] = -1;
					break;
				}
			}
			/* obsb */
			w[FIM][PONTOB(i)] = w[PONTOB(i)][FIM] = dist(fim, obsb[i]);
			for(j = 0; j < num_obstaculos; j++) {
				if(j != i && segments_intersect(fim, obsb[i], obsa[j], obsb[j])) {
					w[FIM][PONTOB(i)] = w[PONTOB(i)][FIM] = -1;
					break;
				}
			}
		}
		/* adjacencias obs-obs */
		for(i = 0; i < num_obstaculos; i++) {
			for(j = 0; j < num_obstaculos; j++) {
				/* a-a */
				if(i == j) continue;
				w[PONTOA(i)][PONTOA(j)] = w[PONTOA(j)][PONTOA(i)] = dist(obsa[i], obsa[j]);
				for(k = 0; k < num_obstaculos; k++) {
					if(k != i && k != j && segments_intersect(obsa[i], obsa[j], obsa[k], obsb[k])) {
						w[PONTOA(i)][PONTOA(j)] = w[PONTOA(j)][PONTOA(i)] = -1;
						break;
					}
				}
				/* b-b */
				w[PONTOB(i)][PONTOB(j)] = w[PONTOB(j)][PONTOB(i)] = dist(obsb[i], obsb[j]);
				for(k = 0; k < num_obstaculos; k++) {
					if(k != i && k != j && segments_intersect(obsb[i], obsb[j], obsa[k], obsb[k])) {
						w[PONTOB(i)][PONTOB(j)] = w[PONTOB(j)][PONTOB(i)] = -1;
						break;
					}
				}
				/* a-b */
				w[PONTOA(i)][PONTOB(j)] = w[PONTOB(j)][PONTOA(i)] = dist(obsa[i], obsb[j]);
				for(k = 0; k < num_obstaculos; k++) {
					if(k != i && k != j && segments_intersect(obsa[i], obsb[j], obsa[k], obsb[k])) {
						w[PONTOA(i)][PONTOB(j)] = w[PONTOB(j)][PONTOA(i)] = -1;
						break;
					}
				}
				/* b-a */
				w[PONTOB(i)][PONTOA(j)] = w[PONTOA(j)][PONTOB(i)] = dist(obsb[i], obsa[j]);
				for(k = 0; k < num_obstaculos; k++) {
					if(k != i && k != j && segments_intersect(obsb[i], obsa[j], obsa[k], obsb[k])) {
						w[PONTOB(i)][PONTOA(j)] = w[PONTOA(j)][PONTOB(i)] = -1;
						break;
					}
				}
			}
		}

		/* output */
		printf("%.2lf\n", dijkstra(INICIO, FIM));
	}

	return 0;
}
Beispiel #10
0
void PatchMatch::patchmatch(cv::Mat &a, cv::Mat &b, BITMAP *&ann, BITMAP *&annd) {
  /* Initialize with random nearest neighbor field (NNF). */
  ann = new BITMAP(a.cols, a.rows);
  annd = new BITMAP(a.cols, a.rows);
  int aew = a.cols - patch_w+1, aeh = a.rows - patch_w + 1;       /* Effective width and height (possible upper left corners of patches). */
  int bew = a.cols - patch_w+1, beh = a.rows - patch_w + 1;
  memset(ann->data, 0, sizeof(int) * a.cols * a.rows);
  memset(annd->data, 0, sizeof(int) * a.cols * a.rows);
  for (int ay = 0; ay < aeh; ay++) {
    for (int ax = 0; ax < aew; ax++) {
      int bx = rand()%bew;
      int by = rand()%beh;
      (*ann)[ay][ax] = XY_TO_INT(bx, by);
      (*annd)[ay][ax] = dist(a, b, ax, ay, bx, by);
    }
  }
  for (int iter = 0; iter < pm_iters; iter++) {
    /* In each iteration, improve the NNF, by looping in scanline or reverse-scanline order. */
    int ystart = 0, yend = aeh, ychange = 1;
    int xstart = 0, xend = aew, xchange = 1;
    if (iter % 2 == 1) {
      xstart = xend-1; xend = -1; xchange = -1;
      ystart = yend-1; yend = -1; ychange = -1;
    }
    for (int ay = ystart; ay != yend; ay += ychange) {
      for (int ax = xstart; ax != xend; ax += xchange) { 
        /* Current (best) guess. */
        int v = (*ann)[ay][ax];
        int xbest = INT_TO_X(v), ybest = INT_TO_Y(v);
        int dbest = (*annd)[ay][ax];

        /* Propagation: Improve current guess by trying instead correspondences from left and above (below and right on odd iterations). */
        if ((unsigned) (ax - xchange) < (unsigned) aew) {
          int vp = (*ann)[ay][ax-xchange];
          int xp = INT_TO_X(vp) + xchange, yp = INT_TO_Y(vp);
          if ((unsigned) xp < (unsigned) bew) {
            improve_guess(a, b, ax, ay, xbest, ybest, dbest, xp, yp);
          }
        }

        if ((unsigned) (ay - ychange) < (unsigned) aeh) {
          int vp = (*ann)[ay-ychange][ax];
          int xp = INT_TO_X(vp), yp = INT_TO_Y(vp) + ychange;
          if ((unsigned) yp < (unsigned) beh) {
            improve_guess(a, b, ax, ay, xbest, ybest, dbest, xp, yp);
          }
        }

        /* Random search: Improve current guess by searching in boxes of exponentially decreasing size around the current best guess. */
        int rs_start = rs_max;
        if (rs_start > MAX(b.cols, b.rows)) { rs_start = MAX(b.cols, b.rows); }
        for (int mag = rs_start; mag >= 1; mag /= 2) {
          /* Sampling window */
          int xmin = MAX(xbest-mag, 0), xmax = MIN(xbest+mag+1,bew);
          int ymin = MAX(ybest-mag, 0), ymax = MIN(ybest+mag+1,beh);
          int xp = xmin+rand()%(xmax-xmin);
          int yp = ymin+rand()%(ymax-ymin);
          improve_guess(a, b, ax, ay, xbest, ybest, dbest, xp, yp);
        }

        (*ann)[ay][ax] = XY_TO_INT(xbest, ybest);
        (*annd)[ay][ax] = dbest;
      }
    }
  }
}
Beispiel #11
0
/* Match image a to image b, returning the nearest neighbor field mapping a => b coords, stored in an RGB 24-bit image as (by<<12)|bx. */
void PatchMatch::patchmatch(cv::Mat &image, cv::Mat &b, BITMAP *ann, BITMAP &annd) {
  /* Initialize with random nearest neighbor field (NNF). */
		cv::Mat a(image.size(), CV_64F);
		image.copyTo(a);
  int aew = a.cols - patch_w+1, aeh = a.rows - patch_w + 1;       /* Effective width and height (possible upper left corners of patches). */
  int bew = b.cols - patch_w+1, beh = b.rows - patch_w + 1;

  for (int ay = 0; ay < aeh; ay++) {
    for (int ax = 0; ax < aew; ax++) {
		int bx, by;
		//if((*ann)[ay][ax] == 0){
		//	  bx = rand() % bew;
		//	  by = rand() % beh;
		//	  (*ann)[ay][ax] = XY_TO_INT(bx, by);
		//}
		//else{
		//	bx = INT_TO_X((*ann)[ay][ax]);
		//	by = INT_TO_Y((*ann)[ay][ax]);
		//}
		bx = rand() % bew;
		by = rand() % beh;
		(*ann)[ay][ax] = XY_TO_INT(bx, by);
		
      annd[ay][ax] = dist(a, b, ax, ay, bx, by);
    }
  }
    int as;
    std::ofstream o_file;
    o_file.open("1.txt");
  for (int iter = 0; iter < pm_iters; iter++) {
    /* In each iteration, improve the NNF, by looping in scanline or reverse-scanline order. */
    int ystart = 0, yend = aeh, ychange = 1;
    int xstart = 0, xend = aew, xchange = 1;
    if (iter % 2 == 1) {
      xstart = xend-1; xend = -1; xchange = -1;
      ystart = yend-1; yend = -1; ychange = -1;
    }

	
//	if(iter == 2)
		as = 0;
    for (int ay = ystart; ay != yend; ay += ychange) {
      for (int ax = xstart; ax != xend; ax += xchange) { 
        /* Current (best) guess. */
		  //if(ax == 366 && ay == 367)
			 // as++;
		//  int sdsd = targetMask[ay * a.cols + ax];
		//if(ax == 352 && ay == 73){
		//	as++;
		//}
        int v = (*ann)[ay][ax];
        int xbest = INT_TO_X(v), ybest = INT_TO_Y(v);
        int dbest = annd[ay][ax];

        /* Propagation: Improve current guess by trying instead correspondences from left and above (below and right on odd iterations). */
		int targetCount = 0;
		for(int i = 0; i < patch_w; i++){
			for(int j = 0; j < patch_w; j++){
				if(targetMask[(ay + i) * a.cols + ax + j] > 0)
					targetCount++;
			}
		}
		if(targetCount > patch_w){
			int vp = (*ann)[ay][ax - xchange];
			int xp = INT_TO_X(vp) + xchange, yp = INT_TO_Y(vp);
			if ((unsigned) xp < (unsigned) bew) {
				xbest = xp;
				ybest = yp;
				(*ann)[ay][ax] = XY_TO_INT(xbest, ybest);
				annd[ay][ax] = annd[ay][ax - xchange];
				continue;
			}
			//else{
			//	vp = (*ann)[ay - ychange][ax];
			//	xp = INT_TO_X(vp), yp = INT_TO_Y(vp) + ychange;
			//	if ((unsigned) yp < (unsigned) beh) {
			//		xbest = xp;
			//		ybest = yp;
			//		(*ann)[ay][ax] = XY_TO_INT(xbest, ybest);
			//		annd[ay][ax] = annd[ay - ychange][ax];
			//		continue;
			//	}
			//}
			if((unsigned) (ay - ychange) < (unsigned) aeh){
				int vp = (*ann)[ay - ychange][ax];
			    int xp = INT_TO_X(vp), yp = INT_TO_Y(vp) + ychange;
			    if ((unsigned) yp < (unsigned) beh) {
			 		improve_guess(a, b, ax, ay, xbest, ybest, dbest, xp, yp);
			    }
			}
		}
		else{
			if ((unsigned) (ax - xchange) < (unsigned) aew) {
			  int vp = (*ann)[ay][ax - xchange];
			  int xp = INT_TO_X(vp) + xchange, yp = INT_TO_Y(vp);
			  if ((unsigned) xp < (unsigned) bew) {
				improve_guess(a, b, ax, ay, xbest, ybest, dbest, xp, yp);
			  }
			}

			if ((unsigned) (ay - ychange) < (unsigned) aeh) {
			  int vp = (*ann)[ay - ychange][ax];
			  int xp = INT_TO_X(vp), yp = INT_TO_Y(vp) + ychange;
			  if ((unsigned) yp < (unsigned) beh) {
				improve_guess(a, b, ax, ay, xbest, ybest, dbest, xp, yp);
			  }
			}
		}

        /* Random search: Improve current guess by searching in boxes of exponentially decreasing size around the current best guess. */
        int rs_start = rs_max;
        if (rs_start > MAX(b.cols, b.rows)) { rs_start = MAX(b.cols, b.rows); }
        for (int mag = rs_start; mag >= 1; mag /= 2) {
            /* Sampling window */
            int xmin = MAX(xbest-mag, 0), xmax = MIN(xbest+mag+1,bew);
            int ymin = MAX(ybest-mag, 0), ymax = MIN(ybest+mag+1,beh);
            int xp = xmin+rand()%(xmax-xmin);
            int yp = ymin+rand()%(ymax-ymin);

		    int targetCount = 0;
			for(int i = 0; i < patch_w; i++){
				for(int j = 0; j < patch_w; j++){
					if(targetMask[(yp + i) * a.cols + xp + j] > 0){
						targetCount++;
						break;
					}
				}
				if(targetCount > 0)
					break;
			}
			if(targetCount > 0){
				mag *= 2;
				continue;
			}
            improve_guess(a, b, ax, ay, xbest, ybest, dbest, xp, yp);
        }

        (*ann)[ay][ax] = XY_TO_INT(xbest, ybest);
        annd[ay][ax] = dbest;
		//a.at<cv::Vec3b>(ay, ax) = b.at<cv::Vec3b>(ybest, xbest);
		
		//for (int dy = 0; dy < patch_w; dy++) {
		//	for (int dx = 0; dx < patch_w; dx++) {
		//		a.at<cv::Vec3b>(ay + dy, ax + dx) = b.at<cv::Vec3b>(ybest + dy, xbest + dx);
		//	}
	 //   }
		if(ay != ybest || ax != xbest)
			as++;

		if(ax == 352 && ay == 73){
			as++;
		}
	//	for(int i = 0; i < cvImage.rows - 7 + 1; i++){
	//	for(int j = 0; j < cvImage.cols - 7 + 1; j++){
	//		int v = (*ann)[i][j];
	//		int x = INT_TO_X(v);
	//		int y = INT_TO_Y(v);
	//		anni.at<cv::Vec3b>(i, j).val[0] = (0+ x) % 256;
	//		anni.at<cv::Vec3b>(i, j).val[1] = (0 + y) % 256;
	//		anni.at<cv::Vec3b>(i, j).val[2] = 0;

	//		if(annd[i][j] > 0){
	//		anndi.at<cv::Vec3b>(i, j).val[0] = annd[i][j] % 256;
	//		}
	//		else 
	//			anndi.at<cv::Vec3b>(i, j).val[0] = 0;
	//		anndi.at<cv::Vec3b>(i, j).val[1] = 0;
	//		anndi.at<cv::Vec3b>(i, j).val[2] = 0;
	//	}
	//}
	//cv::imshow("r", anni);
	//cv::imshow("rd", anndi);
      }
    }
	//std::string s[] = {"0","1","2","3","4","5","6","7","8","9","10","11","12","13","14","15","16","17","18","19"};
	//cv::imshow(s[iter], a);

	o_file<<as<<std::endl;
	std::cout<<as<<std::endl;
  }
  o_file.close();

  a.copyTo(image);
}
Beispiel #12
0
// Select 1 parent and for every activated state all
// economy, research or combat rules have a 25% chance of being replaced.
// Building rules are excluded here both for replacement and as replacement,
// because these could spawn a state change and could possibly corrupt the
// chromosome. Genes in inactivated states are ignored as they are considered
// ‘dead’ and mutation doesn’t really make sense here. 
Chromosome GeneticOperator::RuleReplaceMutation(const Chromosome& parent)
{
	std::vector<State> childStates = parent.getStates();


	// TODO: Only select activated states
	for(size_t i=0;i<childStates.size();i++) {
		State& s = childStates.at(i);
		for (size_t j=0;j<s.getGenes().size(); j++){
			const std::tr1::shared_ptr<Gene> g = s.getGenes().at(j);
			if(typeid(*g) != typeid(BuildGene))
			{				 
				boost::random::uniform_int_distribution<> dist(1, 100);
				int randomNr = dist(randomGen);
				if(randomNr <= 25)
				{
					boost::random::uniform_int_distribution<> dist2(0, 1);
					int replaceNr = dist2(randomGen);
					if(typeid(*g) == typeid(CombatGene))
					{
						if (replaceNr == 1)
						{
							bool found;
							std::tr1::shared_ptr<ResearchGene> gene = StarcraftRules::getValidResearchGene( s, found);
							if (found == true)
								s.replaceGeneAt(j,gene);
						}
						else
						{
							std::tr1::shared_ptr<AttackGene> gene = StarcraftRules::getValidAttackGene(s);
							s.replaceGeneAt(j,gene);
						}
					}
					else if(typeid(*g) == typeid(ResearchGene))
					{
						if (replaceNr == 1)
						{
							std::tr1::shared_ptr<AttackGene> gene = StarcraftRules::getValidAttackGene( s);
							s.replaceGeneAt(j,gene);
						}
						else
						{
							bool found;
							std::tr1::shared_ptr<CombatGene> gene = StarcraftRules::getValidCombatGene( s, found);
							if (found == true)
								s.replaceGeneAt(j,gene);
						}
					}
					else if(typeid(*g) == typeid(AttackGene))
					{
						if (replaceNr == 1)
						{
							bool found;
							std::tr1::shared_ptr<ResearchGene> gene = StarcraftRules::getValidResearchGene( s, found);
							if (found == true)
								s.replaceGeneAt(j,gene);
						}
						else
						{
							bool found;
							std::tr1::shared_ptr<CombatGene> gene = StarcraftRules::getValidCombatGene( s, found);
							if (found == true)
								s.replaceGeneAt(j,gene);
						}
					}

				}
			}
		
		}
	};

	return Chromosome(childStates);
}
Beispiel #13
0
CMapsNetwork::Node* CMapsNetwork::SeekPath(int dwMapID, int dwCol, int dwRow, int dwDesMapID, int dwDesCol, int dwDesRow)
{
	static GatewayCfg from = {};
	from.srcmap = from.desmap = dwMapID;
	from.srcx = from.desx = dwCol;
	from.srcy = from.desy = dwRow;
	
	static GatewayCfg to;
	to.srcmap = to.desmap = dwDesMapID;
	to.srcx = to.desx = dwDesCol;
	to.srcy = to.desy = dwDesRow;

	Node node = {&from}, node2 = {&to};
	node.nCurCost = node2.nCurCost = 0;

	if (dwMapID == dwDesMapID)
	{
		m_pAllNodes[m_nSize] = node;
		m_pAllNodes[m_nSize + 1] = node2;
		m_pAllNodes[m_nSize].pPreNode = &m_pAllNodes[m_nSize + 1];
		return &m_pAllNodes[m_nSize];
	}

	for (int i = 0; i < m_nSize; i++)
	{
		for (size_t j = 0; j < m_pAllNodes[i].vtrNighbors.size(); )
		{
			if (m_pAllNodes[i].vtrNighbors[j].pTo == &m_pAllNodes[m_nSize] || m_pAllNodes[i].vtrNighbors[j].pTo == &m_pAllNodes[m_nSize + 1])
			{
				m_pAllNodes[i].vtrNighbors.erase(m_pAllNodes[i].vtrNighbors.begin() + j);
				continue;
			}
			j++;
		}

		m_pAllNodes[i].pPreNode = NULL;

		if (m_pAllNodes[i].pTransdoor->srcmap == node.pTransdoor->srcmap)
		{
			Node::Link link = {&m_pAllNodes[i], dist(from, *m_pAllNodes[i].pTransdoor)};
			node.vtrNighbors.push_back(link);
		}
		if (m_pAllNodes[i].pTransdoor->desmap == node2.pTransdoor->srcmap)
		{
			Node::Link link = {&m_pAllNodes[m_nSize + 1], dist(*m_pAllNodes[i].pTransdoor, to)};
			m_pAllNodes[i].vtrNighbors.push_back(link);
		}

		m_pAllNodes[i].nCurCost = 0;
	}
	m_pAllNodes[m_nSize] = node;
	m_pAllNodes[m_nSize + 1] = node2;

	list<Node*> lstCurPos;
	list<Node*>::iterator it, itRoundOver;
	lstCurPos.push_back(&m_pAllNodes[m_nSize]);
	Node *pLeastCostNode = NULL;
	itRoundOver = lstCurPos.end();
	itRoundOver--;
	for (it = lstCurPos.begin(); it != lstCurPos.end();)
	{
		vector<Node::Link>::iterator itN;
		for (itN = (*it)->vtrNighbors.begin(); itN != (*it)->vtrNighbors.end(); itN++)
		{
			if (itN->pTo->nCurCost > (*it)->nCurCost + itN->nCost || 
				itN->pTo->nCurCost == 0 && itN->pTo != &m_pAllNodes[m_nSize] && itN->pTo->pPreNode == NULL)
			{
				itN->pTo->nCurCost = (*it)->nCurCost + itN->nCost;
				itN->pTo->pPreNode = *it;
				if (pLeastCostNode == NULL || pLeastCostNode->nCurCost > itN->pTo->nCurCost)
				{
					pLeastCostNode = itN->pTo;
				}

				if (itN->pTo == &m_pAllNodes[m_nSize + 1])
				{
					continue;
				}

				list<Node*>::iterator itTemp;
				for (itTemp = lstCurPos.begin(); itTemp != lstCurPos.end(); itTemp++)
				{
					if (*itTemp == itN->pTo)
					{
						break;
					}
				}
				if (itTemp == lstCurPos.end())
				{
					lstCurPos.push_back(itN->pTo);
				}
			}
		}

		if (itRoundOver == it)
		{
			if (m_pAllNodes[m_nSize + 1].pPreNode && pLeastCostNode && pLeastCostNode->nCurCost >= m_pAllNodes[m_nSize + 1].nCurCost)
			{
				break;
			}
			pLeastCostNode = NULL;
			itRoundOver = lstCurPos.end();
			itRoundOver--;
		}

		//it = lstCurPos.erase(it);
		it++;
	}

	if (m_pAllNodes[m_nSize + 1].pPreNode == NULL)
	{
		return NULL;
	}

	Node *pTemp = &m_pAllNodes[m_nSize + 1];
	Node *pTempNext = NULL;
	Node *pPre = m_pAllNodes[m_nSize + 1].pPreNode;
	while (pTemp)
	{
		pTemp->pPreNode = pTempNext;
		pTempNext = pTemp;
		pTemp = pPre;
		if (pPre)
		{
			pPre = pPre->pPreNode;
		}
	}
	CC_ASSERT(pTempNext == &m_pAllNodes[m_nSize]);
	if (pTempNext != &m_pAllNodes[m_nSize])
	{
		return NULL;
	}
	return pTempNext;
}
Beispiel #14
0
/* hitmm returns 0 (miss), 1 (hit), or 2 (kill) */
int
hitmm(struct monst *magr, struct monst *mdef)
{
	const struct permonst *pa = magr->data, *pd = mdef->data;
	int             didhit;
	schar           tmp;
	boolean         vis;

	if (strchr("Eauy", pa->mlet))
		return (0);
	if (magr->mfroz)
		return (0);	/* riv05!a3 */
	tmp = pd->ac + pa->mlevel;
	if (mdef->mconf || mdef->mfroz || mdef->msleep) {
		tmp += 4;
		if (mdef->msleep)
			mdef->msleep = 0;
	}
	didhit = (tmp > rnd(20));
	if (didhit)
		mdef->msleep = 0;
	vis = (cansee(magr->mx, magr->my) && cansee(mdef->mx, mdef->my));
	if (vis) {
		char            buf[BUFSZ];
		if (mdef->mimic)
			seemimic(mdef);
		if (magr->mimic)
			seemimic(magr);
		(void) snprintf(buf, sizeof(buf), "%s %s", Monnam(magr),
			       didhit ? "hits" : "misses");
		pline("%s %s.", buf, monnam(mdef));
	} else {
		boolean         far = (dist(magr->mx, magr->my) > 15);
		if (far != far_noise || moves - noisetime > 10) {
			far_noise = far;
			noisetime = moves;
			pline("You hear some noises%s.",
			      far ? " in the distance" : "");
		}
	}
	if (didhit) {
		if (magr->data->mlet == 'c' && !magr->cham) {
			magr->mhpmax += 3;
			if (vis)
				pline("%s is turned to stone!", Monnam(mdef));
			else if (mdef->mtame)
				pline("You have a peculiarly sad feeling for a moment, then it passes.");
			monstone(mdef);
			didhit = 2;
		} else if ((mdef->mhp -= d(pa->damn, pa->damd)) < 1) {
			magr->mhpmax += 1 + rn2(pd->mlevel + 1);
			if (magr->mtame && magr->mhpmax > 8 * pa->mlevel) {
				if (pa == &li_dog)
					magr->data = pa = &dog;
				else if (pa == &dog)
					magr->data = pa = &la_dog;
			}
			if (vis)
				pline("%s is killed!", Monnam(mdef));
			else if (mdef->mtame)
				pline("You have a sad feeling for a moment, then it passes.");
			mondied(mdef);
			didhit = 2;
		}
	}
	return (didhit);
}
Beispiel #15
0
int CMath::RandomI(int min,int max)
{
	std::uniform_int<int> dist(min,max);
	return dist(MTRand);
}
void
position_nonref_2allele_test(const snp_pos_info& pi,
                             const blt_options& opt,
                             const bool /*is_always_test*/,
                             nonref_test_call& nrc) {

    static const bool is_mle_freq(false);

    if(pi.ref_base=='N') return;

    // add early escape test here?

    // 1. Determine the two 'primary' alleles -- Simple test just adds
    // up qscores to determine which alleles are primary.
    //
    nrc.nonref_id=(BASE_ID::ANY);
    //unsigned nonref2_id(BASE_ID::ANY); // just ignore this value for now....
    {
        double qtot[N_BASE];
        for(unsigned i(0); i<N_BASE; ++i) qtot[i] = 0;

        const unsigned n_calls(pi.calls.size());
        for(unsigned i(0); i<n_calls; ++i) {
            if(pi.calls[i].base_id==BASE_ID::ANY) continue;
            qtot[pi.calls[i].base_id] += pi.calls[i].get_qscore();
        }

        // get max and max2:
        unsigned max_id=0;
        unsigned max2_id=1;
        for(unsigned b(1); b<N_BASE; ++b) {
            if(qtot[b] > qtot[max_id]) {
                max2_id = max_id;
                max_id = b;
            } else if(qtot[b] > qtot[max2_id]) {
                max2_id = b;
            }
        }

        const unsigned ref_id=base_to_id(pi.ref_base);
        if       (ref_id==max_id) {
            nrc.nonref_id=max2_id;

#if 0
        } else if(ref_id==max2_id) {
            nrc.nonref_id=max_id;
#endif
        } else {
            nrc.nonref_id=max_id;
            //nonref2_id=max2_id;
        }
    }

    blt_float_t lhood[NR2TEST::SIZE];

    lhood[NR2TEST::REF] = calc_pos_nonref_freq_loghood(pi,0.);

    sparse_function sf;
    nonref_allele_freq_loghood_sparse_func nlf(pi,nrc.nonref_id,sf);
    sample_uniform_range(0.,1.,nlf);
    //sample_uniform_range(min_nonref_freq,1.,nlf);

    lhood[NR2TEST::NONREF_MF] = integrate_ln_sparsefunc(sf, opt.min_nonref_freq, 1,1,1);
    lhood[NR2TEST::NONREF_MF_NOISE] = integrate_ln_sparsefunc(sf, 0, opt.nonref_site_error_decay_freq,1,0);

    static const blt_float_t neginf(-std::numeric_limits<blt_float_t>::infinity());
    lhood[NR2TEST::NONREF_OTHER] = neginf;

    //std::cerr << "WAGART: logh ref/nonef: " << lhood[0] << " " << lhood[1] << "\n";

    // TODO: ctor compute this:

    // this goes in here just in case someone cranks both parameters up near 1:
    //
    const double nonref_variant_rate_used = opt.nonref_variant_rate*(1-opt.nonref_site_error_rate);

    blt_float_t prior[NR2TEST::SIZE];
    prior[NR2TEST::REF] = log1p_switch(-(nonref_variant_rate_used+opt.nonref_site_error_rate));
    prior[NR2TEST::NONREF_MF] = std::log(nonref_variant_rate_used/3);
    prior[NR2TEST::NONREF_MF_NOISE] = std::log(opt.nonref_site_error_rate);
    prior[NR2TEST::NONREF_OTHER] = std::log(2*nonref_variant_rate_used/3);

    double pprob[NR2TEST::SIZE];
    for(unsigned i(0); i<NR2TEST::SIZE; ++i) {
        pprob[i] = lhood[i] + prior[i];
    }
    normalize_ln_distro(pprob,pprob+NR2TEST::SIZE,nrc.max_gt);

    nrc.snp_qphred=error_prob_to_qphred(pprob[NR2TEST::REF]+pprob[NR2TEST::NONREF_MF_NOISE]);
    nrc.max_gt_qphred=error_prob_to_qphred(prob_comp(pprob,pprob+NR2TEST::SIZE,nrc.max_gt));

    nrc.is_snp=(nrc.snp_qphred != 0);

    if(! (is_mle_freq && nrc.is_snp)) return;

#if 0
    const double null_loghood(calc_pos_nonref_freq_loghood(pi,0.));

    // heuristic to escape early:
    static const double p_delta(0.001);
    const double delta_loghood(calc_pos_nonref_freq_loghood(pi,p_delta));
    if(null_loghood > delta_loghood) return;

    double x_nonref_freq;
    double x_loghood;

    position_nonref_freq_loghood_minfunc mf(epi);

    static const double x1(0.5);
    static const double x2(0.4);
    codemin::minimize_1d(x1,x2,mf.val(x1),mf,x_nonref_freq,x_loghood);

    x_nonref_freq = mf.arg_to_prob(x_nonref_freq);

    const double log_lrt(-2.*(x_loghood+null_loghood));

    // becuase null has the parameter fixed to a boundary value, the
    // asymmtotic distribution is a 50:50 mixture of csq(0) and chq(1)
    // -- the same effect as multiplying alpha of csq(1) by 2, dividing
    // the null prob by 2. (as we do below):
    boost::math::chi_squared dist(1);
    const double null_prob((1.-boost::math::cdf(dist,log_lrt))/2.);

    sc.is_snp=(null_prob<alpha);
    sc.null_loghood=null_loghood;
    sc.min_test_loghood=-x_loghood;
    sc.snp_prob=1.-null_prob;

    // if it's a snp then get additional information on non-reference
    // allele frequencies.
    //
    if(not sc.is_snp) return;

    static const double line_tol(1e-7);
    static const double start_ratio(0.05);
    static const double min_start_dist(1e-6);
    static const double end_tol(1e-7);
    static const unsigned max_iter(200);

    const unsigned ref_base_id(base_to_id(pi.ref_base));

    const double ref_freq(1.-x_nonref_freq);
    const double nonref_freq((x_nonref_freq)/3.);
    for(unsigned i(0); i<N_BASE; ++i) {
        if(i==ref_base_id) sc.allele_freq[i] = ref_freq;
        else               sc.allele_freq[i] = nonref_freq;
    }

    static const unsigned N_BASE2(N_BASE*N_BASE);
    double conj_dir[N_BASE2];
    std::fill(conj_dir,conj_dir+N_BASE2,0.);
    for(unsigned i(0); i<N_BASE; ++i) {
        const double start_dist( std::max(std::fabs(sc.allele_freq[i]*start_ratio),min_start_dist) );
        conj_dir[i*(N_BASE+1)] = start_dist;
    }

    double start_tol(end_tol);
    unsigned iter;
    double x_all_loghood;
    double final_dlh;
    position_allele_distro_loghood_minfunc alm(epi);
    codemin::minimize_conj_direction(sc.allele_freq,conj_dir,alm,start_tol,end_tol,line_tol,
                                     x_all_loghood,iter,final_dlh,max_iter);
    alm.arg_to_prob(sc.allele_freq,sc.allele_freq);

    sc.min_loghood=-x_all_loghood;
#endif
}
Beispiel #17
0
double tour_length(struct point p[MAX_N], int n, int tour[MAX_N]) {
    int i;
    double sum=0.0;
    for(i=0;i<n;i++) sum+=dist(p[tour[i]],p[tour[(i+1)%n]]);
    return sum;// 総距離が関数の戻り値
}
Beispiel #18
0
void GeoConnection::connectLikeInVC_Block(NeuronID lo_row, NeuronID hi_row, NeuronID lo_col, NeuronID hi_col, bool skip_diag)
{
    int r = 0; // these variables are used to speed up building the matrix if the destination is distributed
    int s = 1;

    r = communicator->rank() - dst->get_locked_rank();
    s = dst->get_locked_range();

    // Create uniform int generator
    boost::random::uniform_int_distribution<> dist(1,100);
    boost::variate_generator<boost::mt19937&, boost::random::uniform_int_distribution<> > die(GeoConnection::sparse_connection_gen, dist);

    if (!has_been_allocated)
        throw AurynConnectionAllocationException();

    AurynLong idim = (hi_row - lo_row);
    AurynLong jdim = (hi_col - lo_col) / s;

    if ( (hi_col - lo_col) % s > r ) { // some ranks have one more "carry" neuron
        jdim += 1;
    }

    AurynLong x = 0;
    AurynLong stop = idim*jdim;
    AurynLong count = 0;
    NeuronID i = 0;
    NeuronID j = 0;
    AurynWeight temp_weight = 0;
    while ( x < stop ) {

        // calculate i and j position
        i = lo_row + x / jdim;
        j = lo_col + s*(x % jdim) + r;

        double p = getProbability(i,j,sigma)*100;
        double ps = die();

        if ( (j >= lo_col) && (!skip_diag || i!=j) && p > ps) {

            try {
                temp_weight = (same_orientation(i,j))? oWeight: cWeight;
                if ( push_back(i,j,temp_weight) )
                    count++;
            }
            catch ( AurynMatrixDimensionalityException )
            {
                stringstream oss;
                oss << "GeoConnection: ("
                    << get_name()
                    <<"): Trying to add elements outside of matrix (i="
                    << i
                    << "j="
                    << j
                    << ", "
                    << count
                    << "th element) ";
                logger->msg(oss.str(),ERROR);
                return;
            }
            catch ( AurynMatrixPushBackException )
            {
                stringstream oss;
                oss << "GeoConnection: ("<< get_name()
                    << "): Failed pushing back element. Maybe due to out of order pushing? "
                    << " (" << i << "," << j << ") "
                    << " with count=" << count
                    << " in connect_block_random ( fill_level= " << w->get_fill_level() << " )";
                logger->msg(oss.str(),ERROR);
                return;
            }
            catch ( AurynMatrixBufferException )
            {
                stringstream oss;
                oss << "GeoConnection: ("
                    << get_name()
                    <<"): Buffer full after pushing "
                    << count
                    << " elements."
                    << " There are pruned connections!";
                logger->msg(oss.str(),ERROR);
                return;
            }
        }

        x++;

    }

    stringstream oss;
    oss << "GeoConnection: ("<< get_name() <<"): Finished connectLikeInVC ["
        << lo_row << ", " << hi_row << ", " << lo_col << ", " << hi_col <<  "] " << " (stop count "
        << std::scientific << setprecision(4) << (double) stop
        << ") and successfully pushed " << (double) count <<  " entries. "
        << "Resulting overall sparseness " << 1.*get_nonzero()/src->get_pre_size()/dst->get_post_size()
        << " Size of w: " << get_nonzero();

    logger->msg(oss.str(),DEBUG);
}
Beispiel #19
0
void Surf::InitMesh( vector< ISegChain* > chains )
{

//static int name_cnt = 0;
//char str[256];
//sprintf( str, "Surf_UW%d.dat", name_cnt );
//name_cnt++;
//FILE* fp = fopen( str, "w" );
//int ccnt = 0;
//for ( int i = 0 ; i < (int)chains.size() ; i++ )
//{
//	if ( ccnt%5 == 0 ) fprintf( fp, "COLOR RED\n" );
//	else if ( ccnt % 5 == 1 ) fprintf( fp, "COLOR BLUE\n" );
//	else if ( ccnt % 5 == 2 ) fprintf( fp, "COLOR GREEN\n" );
//	else if ( ccnt % 5 == 3 ) fprintf( fp, "COLOR PURPLE\n" );
//	else if ( ccnt % 5 == 4 ) fprintf( fp, "COLOR YELLOW\n" );
//	fprintf( fp, "MOVE \n" );
//	ccnt++;
//		for ( int j = 1 ; j < (int)chains[i]->m_TessVec.size() ; j++ )
//		{
//			vec2d uw0 = chains[i]->m_TessVec[j-1]->GetPuw( this )->m_UW;
//			vec2d uw1 = chains[i]->m_TessVec[j]->GetPuw( this )->m_UW;
//			fprintf( fp, "%f %f\n", uw0[0], uw0[1] );
//			fprintf( fp, "%f %f\n", uw1[0], uw1[1] );
//		}
//}
//fclose(fp);

	//==== Store Only One Instance of each IPnt ====//
	set< IPnt* > ipntSet;
	for ( int i = 0 ; i < (int)chains.size() ; i++ )
		for ( int j = 0 ; j < (int)chains[i]->m_TessVec.size() ; j++ )
		{
			ipntSet.insert( chains[i]->m_TessVec[j] );
		}

	vector < vec2d > uwPntVec;

	set< IPnt* >::iterator ip;
	for ( ip = ipntSet.begin() ; ip != ipntSet.end() ; ip++ )
	{
		vec2d uw = (*ip)->GetPuw( this )->m_UW;

		int min_id;
		double min_dist = 1.0;
		for ( int i = 0 ; i < (int)uwPntVec.size() ; i++ )
		{
			double d = dist( uwPntVec[i], uw );
			if ( d < min_dist )
			{
				min_dist = d;
				min_id = i;
			}
		}

		if ( min_dist < 0.0001 )
		{
			(*ip)->m_Index = min_id;
		}
		else
		{
			uwPntVec.push_back( uw );
			(*ip)->m_Index = uwPntVec.size()-1;
		}
	}


	MeshSeg seg;
	vector< MeshSeg > isegVec;
	for ( int i = 0 ; i < (int)chains.size() ; i++ )
		for ( int j = 1 ; j < (int)chains[i]->m_TessVec.size() ; j++ )
		{
			seg.m_Index[0] = chains[i]->m_TessVec[j-1]->m_Index;
			seg.m_Index[1] = chains[i]->m_TessVec[j]->m_Index;
			isegVec.push_back( seg );
		}

//	////jrg Check For Duplicate Segs
//	vector< MeshSeg > dupMeshSegVec;
//	for ( int i = 0 ; i < (int)isegVec.size() ; i++ )
//	{
//		int iind0 = isegVec[i].m_Index[0];
//		int iind1 = isegVec[i].m_Index[1];
//		bool dup = false;
//		for ( int j = i+1 ; j < (int)isegVec.size() ; j++ )
//		{
//			int jind0 = isegVec[j].m_Index[0];
//			int jind1 = isegVec[j].m_Index[1];
//			if ( (iind0 == jind0 && iind1 == jind1) || 
//				 (iind0 == jind1 && iind1 == jind0) )
//			{
//				dup = true;
//				//printf("Surf: Duplicate Seg \n" );
//			}
//		}
//		if ( !dup )
//			dupMeshSegVec.push_back( isegVec[i] );
//	}
//	isegVec = dupMeshSegVec;


////jrg - Check For Point Close to Other Segs
//vector< MeshSeg > newMeshSegVec;
//for ( int i = 0 ; i < (int)isegVec.size() ; i++ )
//{
//	int iind0 = isegVec[i].m_Index[0];
//	int iind1 = isegVec[i].m_Index[1];
//
//	for ( int j = 0 ; j < (int)uwPntVec.size() ; j++ )
//	{
//		if ( j != iind0 && j != iind1 )
//		{
//			vec2d proj = proj_pnt_on_line_seg(uwPntVec[iind0], uwPntVec[iind1], uwPntVec[j] );
//			double d = dist( proj, uwPntVec[j] );
//
//			if ( d < 0.000001 )
//			{
//				MeshSeg addseg1, addseg2;
//				addseg1.m_Index[0] = iind0;
//				addseg1.m_Index[1] = j;
//				addseg2.m_Index[0] = j;
//				addseg2.m_Index[1] = iind1;
//				newMeshSegVec.push_back( addseg1 );
//				newMeshSegVec.push_back( addseg2 );
//
//				//printf("Surf: %d Proj Pnt Dist = %f\n", (int)this, d );
//				//printf("  Seg = %f %f   %f %f\n", 
//				//	uwPntVec[iind0].x(),  uwPntVec[iind0].y(), 
//				//	uwPntVec[iind1].x(),  uwPntVec[iind1].y());
//				//vec3d p = CompPnt( uwPntVec[j].x(), uwPntVec[j].y() );
//				//printf("  Pnt uv = %f %f    %f %f %f\n", uwPntVec[j].x(), uwPntVec[j].y(), p.x(), p.y(), p.z() );
//			}
//			else
//			{
//				newMeshSegVec.push_back( isegVec[i] );
//			}
//		}
//	}
//}

//vector< MeshSeg > smallMeshSegVec;
//for ( int i = 0 ; i < (int)isegVec.size() ; i++ )
//{
//	int ind0 = isegVec[i].m_Index[0];
//	int ind1 = isegVec[i].m_Index[1];
//	double d = dist( uwPntVec[ind0], uwPntVec[ind1] );
//	if ( d > 0.00000001 )
//	{
//		smallMeshSegVec.push_back( isegVec[i] );
//	}
//	else
//		printf("Surf: %d Small Seg Dist = %12.12f\n", (int)this, d );
//}
//isegVec = smallMeshSegVec;


	m_Mesh.InitMesh( uwPntVec, isegVec );
}
Beispiel #20
0
inline double zeroDist(double x, double y){
    return dist(0, 0, x, y);
}
Beispiel #21
0
unsigned Rng::getRandom(unsigned lower, unsigned upper)
{
	std::tr1::uniform_int<unsigned> dist(lower, upper);
  return dist(pImpl_->generator_);
}
Beispiel #22
0
//==== Update Method ===//
void SSControlSurf::Update()
{
    // Build Control Surface as a rectangle with the points counter clockwise

    vec3d c_uws_upper, c_uws_lower, c_uwe_upper, c_uwe_lower;
    vector< vec3d > pnt_vec;
    double u, w;

    Geom* geom = VehicleMgr.GetVehicle()->FindGeom( m_CompID );
    if ( !geom ) { return; }

    VspSurf* surf = geom->GetSurfPtr();
    if ( !surf ) { return; }


    VspCurve startcrv;
    surf->GetU01ConstCurve( startcrv, m_UStart() );

    piecewise_curve_type c = startcrv.GetCurve();

    double vmin = c.get_parameter_min(); // Really must be 0.0
    double vmax = c.get_parameter_max(); // Really should be 4.0

    double vle = ( vmin + vmax ) * 0.5;

    double vtelow = vmin + TMAGIC;
    double vteup = vmax - TMAGIC;

    curve_point_type te, le;
    te = c.f( vmin );
    le = c.f( vle );

    double chord, d;
    chord = dist( le, te );

    if ( m_AbsRelFlag() == vsp::REL )
    {
        d = chord * m_StartLenFrac();
        m_StartLength.Set( d );
    }
    else
    {
        d = m_StartLength();
        m_StartLenFrac.Set( d / chord );
    }

    if ( m_ConstFlag.Get() )
    {
        if ( m_AbsRelFlag() == vsp::REL )
        {
            m_EndLenFrac.Set( d / chord );
        }
        else
        {
            m_EndLength.Set( d );
        }
    }

    // Mid-curve points on upper and lower surface.  To serve as initial guess.
    double vlowmid, vupmid;

    vlowmid = vtelow + m_StartLenFrac() * ( vle - vtelow );
    vupmid = vle + ( 1.0 - m_StartLenFrac() ) * ( vteup - vle );


    curve_point_type telow, teup;
    telow = c.f( vtelow );
    teup = c.f( vteup );

    piecewise_curve_type clow, cup;
    c.split( clow, cup, vle );


    double vlow, vup;

    if ( m_SurfType() != LOWER_SURF )
    {
        eli::geom::intersect::specified_distance( vup, cup, teup, d, vupmid );
        c_uws_upper = vec3d( m_UStart(), vup / vmax, 0 );
    }

    if ( m_SurfType() != UPPER_SURF )
    {
        eli::geom::intersect::specified_distance( vlow, clow, telow, d, vlowmid );
        c_uws_lower = vec3d( m_UStart(), vlow / vmax, 0 );
    }


    VspCurve endcrv;
    surf->GetU01ConstCurve( endcrv, m_UEnd() );
    c = endcrv.GetCurve();

    te = c.f( vmin );
    le = c.f( vle );

    chord = dist( le, te );


    if ( m_AbsRelFlag() == vsp::REL )
    {
        d = chord * m_EndLenFrac();
        m_EndLength.Set( d );
    }
    else
    {
        d = m_EndLength();
        m_EndLenFrac.Set( d / chord );
    }

    vlowmid = vtelow + m_EndLenFrac() * ( vle - vtelow );
    vupmid = vle + ( 1.0 - m_EndLenFrac() ) * ( vteup - vle );

    telow = c.f( vtelow );
    teup = c.f( vteup );

    c.split( clow, cup, vle );

    if ( m_SurfType() != LOWER_SURF )
    {
        eli::geom::intersect::specified_distance( vup, cup, teup, d, vupmid );
        c_uwe_upper = vec3d( m_UEnd(), vup / vmax, 0 );
    }

    if ( m_SurfType() != UPPER_SURF )
    {
        eli::geom::intersect::specified_distance( vlow, clow, telow, d, vlowmid );
        c_uwe_lower = vec3d( m_UEnd(), vlow / vmax, 0 );
    }

    // Build Control Surface

    if ( m_SurfType() == UPPER_SURF )
    {
        pnt_vec.resize( 4 );
        pnt_vec[0] = vec3d( m_UStart(), 1, 0 );
        pnt_vec[1] = c_uws_upper;
        pnt_vec[2] = c_uwe_upper;
        pnt_vec[3] = vec3d( m_UEnd(), 1, 0 );
    }
    else if ( m_SurfType() == LOWER_SURF )
    {
        pnt_vec.resize( 4 );
        pnt_vec[0] = vec3d( m_UStart(), 0, 0 );
        pnt_vec[1] = c_uws_lower;
        pnt_vec[2] = c_uwe_lower;
        pnt_vec[3] = vec3d( m_UEnd(), 0, 0 );
    }
    else
    {
        pnt_vec.resize( 8 );
        pnt_vec[0] = vec3d( m_UStart(), 1, 0 );
        pnt_vec[1] = c_uws_upper;
        pnt_vec[2] = c_uwe_upper;
        pnt_vec[3] = pnt_vec[3] = vec3d( m_UEnd(), 1, 0 );
        pnt_vec[4] = vec3d( m_UEnd(), 0, 0 );
        pnt_vec[5] = c_uwe_lower;
        pnt_vec[6] = c_uws_lower;
        pnt_vec[7] = vec3d( m_UStart(), 0, 0 );
    }
    //  pnt_vec[3] = pnt_vec[0];

    int pind = 0;
    int num_segs = pnt_vec.size() - 1;

    if ( m_SurfType() == BOTH_SURF )
    {
        num_segs--;
    }


    m_LVec.resize( num_segs );

    for ( int i = 0; i < num_segs; i++ )
    {
        if ( m_SurfType() == BOTH_SURF && i == 3 )
        {
            pind++;
        }
        m_LVec[i].SetSP0( pnt_vec[pind] );
        pind++;
        m_LVec[i].SetSP1( pnt_vec[pind] );
        m_LVec[i].Update( geom );
    }

    SubSurface::Update();
}
Beispiel #23
0
bool
Plane::inside(const Vector &pos) const
{
    return dist(pos)>=0.0;
}
Beispiel #24
0
bool Bullet::update(Server& s, QList<Unit*>& plrs)
{
	double v = sqrt(vx*vx + vy*vy);
	double len = v * FRAME_TIME;

	double ex = x + vx * FRAME_TIME;
	double ey = y + vy * FRAME_TIME;
	double vx0=vx/v, vy0=vy/v;
	int nearest=-1;
	double nDist=1e200;
	for(int i=0; i<plrs.size(); ++i) {
		double px = plrs[i]->x, py = plrs[i]->y;
		double d;
		if ((d=distToPlayer(x,y,px,py,vx0,vy0))<nDist && dist(x,y,ex,ey,px,py) < PLAYER_RADIUS) {
			nearest = i;
			nDist = d;
		}
	}

#if 0
	Area& a = s.area;
	int ix=x, iy=y;
	int idx=vx<0?-1:1, idy=vy<0?-1:1;
	double x0=x,y0=y;
	double d2;
	while((d2=(x-x0)*(x-x0) + (y-y0)*(y-y0)) <= len*len) {
		if (d2 >= nDist*nDist) {
			s.hitPlayer(*plrs[nearest], type);
			return 1;
		}
		if (a.blocked(ix,iy)) return 1;
		int ix2=ix+idx, iy2=iy+idy;
		double dx = idx<0 ? x-ix : ix2-x;
		double dy = idy<0 ? y-iy : iy2-y;
		double xx = dy*fabs(vx0/vy0);
		double yy = dx*fabs(vy0/vx0);
		if (yy>dy) {
			y=idy<0?iy:iy2;
			iy=iy2;
			x += vx0*xx;
		} else {
			x=idx<0?ix:ix2;
			ix=ix2;
			y += vy0*yy;
		}
	}
	if (nDist < len) {
		s.hitPlayer(*plrs[nearest], type);
		return 1;
	}
	x=ex,y=ey;
	return 0;
#else
	if (nDist < len) {
		ex = x + vx0*nDist;
		ey = y + vy0*nDist;
	}
	QPointF p = getWallHitPoint(x,y,ex,ey,s.area);
	x=p.x(), y=p.y();
	if (nDist<len && fabs(x-ex)<1e-3 && fabs(y-ey)<1e-3) {
		s.bulletHit(plrs[nearest], *this);
		return 1;
	}
	if (fabs(x-ex)>1e-3 || fabs(x-ex)>1e-3) {
		x -= 1e-3*vx0;
		y -= 1e-3*vy0;
		s.bulletHit(0, *this);
		return 1;
	}
	return 0;
#endif
}
Beispiel #25
0
int main (int argc, char *argv[])
{
  int ghost[3],pacman[3], red[3], target[2], number_ghost;
  char maze[L][C];
  char ch;
  int i=0,j=0;
  FILE *fp;
  fp = fopen("maze.txt","r" );
  if(fp == NULL)
	printf("Erro, nao foi possivel abrir o arquivo\n");
  else
	while( (ch=fgetc(fp))!= EOF )
	{
		if (ch != '\n')
		{
			maze[i][j] = ch;
			j++;
		}else
		{
			j = 0;
			i++;
		}
	}		
  fclose(fp);
	
  clear_screen();
  print_maze(maze);
 
  printf ( "\t>>> GHOSTS <<<\n" );
  printf ( "1.BLINK  2.PINK  3.INKY  4.CLYDE\n" );
  do
  {  	  
  	  printf ( "Qual o numero do Ghost? = " );
          scanf ("%d", &number_ghost);
  }while(number_ghost > 4 || number_ghost < 1 );  
  //number_ghost = 2;
  do
  {
  	print_maze(maze);
	printf ( "Qual a posição do GHOST? (x,y)\n" );
  	printf ( "x: " );
  	scanf ( "%d", &ghost[1] ); // referente a coluna - ghost
  	printf ( "y: " );
  	scanf ( "%d", &ghost[0] ); // referente a linha - ghost
	printf ( "Direção do ghost = 1-Down , 2-Right, 3-Up, 4-Left = " );
        scanf ("%d", &ghost[2]);
	clear_screen();
  }while (!validate_position(maze,ghost[0],ghost[1]) || ghost[2] > 4 ||ghost[2] < 1);                                           //mudar os parametros
	position (maze,ghost,'3');
    
    
   do
  {
  	print_maze(maze);
	printf ( "Qual a posição do RED? (x,y)\n" );
  	printf ( "x: " );
  	scanf ( "%d", &red[1] ); // referente a coluna - ghost
  	printf ( "y: " );
  	scanf ( "%d", &red[0] ); // referente a linha - ghost
	printf ( "Direção do red = 1-Down , 2-Right, 3-Up, 4-Left = " );
        scanf ("%d", &red[2]);
	clear_screen();
  }while (!validate_position(maze,red[0],red[1]) || red[2] > 4 ||red[2] < 1);                                           //mudar os parametros
	position (maze,red,'4');
    
    
  do 
  {
	clear_screen();
  	print_maze (maze);
  	printf ( "Qual a posição do Pacman? (x,y)\n" );
  	printf ( "x: " );
  	scanf ( "%d", &pacman[1]); // referente a coluna - ghost
  	printf ( "y: " );
  	scanf ( "%d", &pacman[0]); // referente a linha - ghost
  }while (!validate_position(maze,pacman[0],pacman[1]));                          //mudar os parametros
  
  do
  {  	
        printf ( "Direção do Pac-mam: 1-Down , 2-Right, 3-Up, 4-Left =  " );
        scanf ("%d",&pacman[2]);
  }while( pacman[2]>4 ||pacman[2]<1);
  position (maze,pacman,'5' );//mudar os parametros
  
  dist(pacman);
  //marca_target aqui// calcular o target aqui e passa-lo para a walking
  //target[0]=(pacman[0]-red[0])+pacman[0];
  //target[1]=(pacman[1]-red[1])+pacman[1];
  //verifica_target(maze, target);
  calcula_target(maze, red, pacman, target);
  position(maze, target, '8');
  print_maze (maze);
  walking(maze,ghost,target);//mudar os parametros
  print_maze (maze);
  
  return 0;
}
Beispiel #26
0
void voronoi(int triangulate, Site * (*nextsite) (void))
{
    Site *newsite, *bot, *top, *temp, *p;
    Site *v;
    Point newintstar;
    char pm;
    Halfedge *lbnd, *rbnd, *llbnd, *rrbnd, *bisector;
    Edge *e;

    edgeinit();
    siteinit();
    PQinitialize();
    bottomsite = (*nextsite) ();
#ifdef STANDALONE
    out_site(bottomsite);
#endif
    ELinitialize();

    newsite = (*nextsite) ();
    while (1) {
	if (!PQempty())
	    newintstar = PQ_min();

	if (newsite != (struct Site *) NULL && (PQempty()
						|| newsite->coord.y <
						newintstar.y
						|| (newsite->coord.y ==
						    newintstar.y
						    && newsite->coord.x <
						    newintstar.x))) {
	    /* new site is smallest */
#ifdef STANDALONE
	    out_site(newsite);
#endif
	    lbnd = ELleftbnd(&(newsite->coord));
	    rbnd = ELright(lbnd);
	    bot = rightreg(lbnd);
	    e = bisect(bot, newsite);
	    bisector = HEcreate(e, le);
	    ELinsert(lbnd, bisector);
	    if ((p = hintersect(lbnd, bisector)) != (struct Site *) NULL) {
		PQdelete(lbnd);
		PQinsert(lbnd, p, dist(p, newsite));
	    }
	    lbnd = bisector;
	    bisector = HEcreate(e, re);
	    ELinsert(lbnd, bisector);
	    if ((p = hintersect(bisector, rbnd)) != (struct Site *) NULL)
		PQinsert(bisector, p, dist(p, newsite));
	    newsite = (*nextsite) ();
	} else if (!PQempty()) {
	    /* intersection is smallest */
	    lbnd = PQextractmin();
	    llbnd = ELleft(lbnd);
	    rbnd = ELright(lbnd);
	    rrbnd = ELright(rbnd);
	    bot = leftreg(lbnd);
	    top = rightreg(rbnd);
#ifdef STANDALONE
	    out_triple(bot, top, rightreg(lbnd));
#endif
	    v = lbnd->vertex;
	    makevertex(v);
	    endpoint(lbnd->ELedge, lbnd->ELpm, v);
	    endpoint(rbnd->ELedge, rbnd->ELpm, v);
	    ELdelete(lbnd);
	    PQdelete(rbnd);
	    ELdelete(rbnd);
	    pm = le;
	    if (bot->coord.y > top->coord.y) {
		temp = bot;
		bot = top;
		top = temp;
		pm = re;
	    }
	    e = bisect(bot, top);
	    bisector = HEcreate(e, pm);
	    ELinsert(llbnd, bisector);
	    endpoint(e, re - pm, v);
	    deref(v);
	    if ((p = hintersect(llbnd, bisector)) != (struct Site *) NULL) {
		PQdelete(llbnd);
		PQinsert(llbnd, p, dist(p, bot));
	    }
	    if ((p = hintersect(bisector, rrbnd)) != (struct Site *) NULL) {
		PQinsert(bisector, p, dist(p, bot));
	    }
	} else
	    break;
    }

    for (lbnd = ELright(ELleftend); lbnd != ELrightend;
	 lbnd = ELright(lbnd)) {
	e = lbnd->ELedge;
	clip_line(e);
#ifdef STANDALONE
	out_ep(e);
#endif
    }
}
Beispiel #27
0
double Scheduler::randomExponentialInterval(double mean) {
  static std::mt19937* rng = new std::mt19937();
  std::uniform_real_distribution<double> dist(0, 1.0);
  /* Cap the lower end so that we don't return infinity */
  return - log(std::max(dist(*rng), 1e-9)) * mean;
}
Beispiel #28
0
float CMath::RandomF(float min,float max)
{
	std::uniform_real<float> dist(min,max);
	return dist(MTRand);
}
Beispiel #29
0
void trimesh_t::add_centroid_to_object_distance() {
	list<trimesh_node_t*>::iterator triangle_iterator;
	vector_t centroid, normal, start_point, mid_point, end_point;
	int punt, start_val, i;

	float search_distance = (xstep+ystep+zstep);

	for (triangle_iterator = triangles.begin(); triangle_iterator != triangles.end(); triangle_iterator++) {
		if ((*triangle_iterator)->dirty) {

			// Compute location of centroid


	centroid = triangle_centroid(*(*triangle_iterator)->verticies[0], *(*triangle_iterator)->verticies[1], *(*triangle_iterator)->verticies[2]);	
	normal = (*triangle_iterator)->normal;

	// See if the centroid is on the object
	punt = 0;
	if (octree->eval_at_point(centroid.x, centroid.y, centroid.z)) {
		// If so, follow the normal to the boundary
		end_point = add(centroid, mul(normal, search_distance));
		// Is the proposed end point also on the object?

		if (octree->eval_at_point(end_point.x, end_point.y, end_point.z)) 
			punt = 1;
		
		start_val = 1;
	}
	else {
		// If not, follow the antinormal to the boundary
		end_point = add(centroid, mul(normal, -search_distance));
		// Is the proposed end point also not the object?
		if (!(octree->eval_at_point(end_point.x, end_point.y, end_point.z))) 
			punt = 1;

	
		start_val = 0;
	}
	// At this point, we have an end point for our search, and know that the 
	// object boundary lies somewhere in beteeen.  Binary search out the object position
	
	if (punt == 0) {
	
	start_point = centroid;

	for(i=0; i<8; i++) {
		mid_point = midpoint(start_point, end_point);
		if (octree->eval_at_point(mid_point.x, mid_point.y, mid_point.z) == start_val) {
			start_point = mid_point;
		}
		else {
			end_point = mid_point;
		}
	}
	mid_point = midpoint(start_point, end_point);

	if (start_val)
		(*triangle_iterator)->centroid_to_object = dist(mid_point, centroid);
	else
		(*triangle_iterator)->centroid_to_object = -dist(mid_point, centroid);

	}
				
		}
	}

}
/*! @brief Perfroms the mean shift operation and then returns the labelled clusters and the location of the means
 * 
 */
void ossimSDFilter::meanshift(std::vector< double >& data, int rows, int cols, double bw, double rate, int iterMax, std::vector< double >& labelledClusters, std::vector< double >& meansFinal)
{
  float bw2 = bw * bw;      
    int i = 0, j = 0;          
    int delta = 1;
    int nLabelledClusters = 1;
    double epsilon = 0.0001;
    
    std::vector<double> meansCurr;
    std::vector<double> meansNext;
    std::vector<double> meansRow;
    std::vector<int> groupedLabels;
    std::vector<int> deltas;
    std::vector<double> originalData;
    
    //Initialise vectors
    meansCurr.insert(meansCurr.end(),&data[0], &data[rows*cols]);
    meansNext.resize(rows*cols);
    meansRow.resize(rows);
    groupedLabels.resize(cols);
    deltas.resize(cols);
    originalData.resize(rows*cols);
    originalData = meansCurr;
    
    //Fill deltas with 1s and labels with 0
    std::fill(deltas.begin(),deltas.end(),1);  
    std::fill(groupedLabels.begin(), groupedLabels.end(), 0);
    std::fill(labelledClusters.begin(), labelledClusters.begin(), 0);
    
    for(int itercount = 0;itercount < iterMax && delta; itercount++)
    {
      
      //Calculate next mean by shifting current data
      i = 0;
      for(std::vector<int>::iterator itCurrentDelta = deltas.begin(); itCurrentDelta != deltas.end(); ++itCurrentDelta, ++i)      
      {
	if(*itCurrentDelta > 0)
	{
	  std::vector<double> subVectorCurr(meansCurr.begin()+i*rows,meansCurr.end());
	  if(meanvector(subVectorCurr, originalData, rows, cols, bw2, meansRow) > 0) 
	  {
	   j = 0; 
           for(std::vector<double>::iterator itCurrentRowMean = meansRow.begin(); itCurrentRowMean != meansRow.end(); ++itCurrentRowMean, ++j)
	     meansNext.at(i*rows+j) = (1-rate)*meansCurr.at(i*rows+j) + rate*(*itCurrentRowMean);
          } 
          else
	  {
	   j = 0; 
           for(std::vector<double>::iterator itCurrentRowMean = meansRow.begin(); itCurrentRowMean != meansRow.end(); ++itCurrentRowMean, ++j)
	     meansNext.at(i*rows+j) = meansCurr.at(i*rows+j);
          }
 	}
      }

      delta = 0;
      i = 0;
      for(std::vector<int>::iterator itCurrentDelta = deltas.begin(); itCurrentDelta != deltas.end(); ++itCurrentDelta, ++i)
      {
         std::vector<double> subVectorCurr(meansCurr.begin()+i*rows,meansCurr.end());
         std::vector<double> subVectorNext(meansNext.begin()+i*rows,meansNext.end());
	 if(*itCurrentDelta > 0 && dist(subVectorNext,subVectorCurr) > epsilon)
          delta = 1;
         else
          *itCurrentDelta = 0;
      }
	meansCurr = meansNext;
    }
    
    
    i = 0;
    for(std::vector<int>::iterator itCurrentLabel = groupedLabels.begin(); itCurrentLabel != groupedLabels.end(); ++itCurrentLabel, ++i)
    {
       if(*itCurrentLabel == 0)
       {
	 j = 0;
	 for(std::vector<int>::iterator itCurrentLabelInner = groupedLabels.begin(); itCurrentLabelInner != groupedLabels.end(); ++itCurrentLabelInner, ++j)
	 {
	  std::vector<double> subVectorCurr(meansCurr.begin()+j*rows,meansCurr.end());
	  std::vector<double> subVectorNext(meansNext.begin()+i*rows,meansNext.end());
   	  
	  if(*itCurrentLabelInner == 0 && dist(subVectorNext,subVectorCurr) < bw2)
          {
            labelledClusters.at(j) = nLabelledClusters;
            groupedLabels.at(j) = 1;
          }  
	 }
         nLabelledClusters++;
    	}
    }
    
    nLabelledClusters = nLabelledClusters-1;
    meansFinal = meansNext; 
}