C++ Dist函数代码示例

FloatType route_evaluation(GlobalGiftData g_data, const Route & r){
    FloatType res = 0.0;
    int test_counter = 0;

    FloatType w = sleigh_base_weight;
    for (int i = 0; i < r.size(); ++i) {
        const auto & gift = g_data[r[i]];
        w += gift.Weight();
        ++test_counter;
    }

    auto w_limit = sleigh_weight_limit + sleigh_base_weight;

    // weight > then limit
    assert(w <= w_limit);

    res += w*Dist(north_pole, g_data[r[0]].Loc() );

    test_counter = 0;
    int __size = r.size();
    for (int i = 0; i < __size-1; ++i) {
        const auto & gift = g_data[r[i]];
        const auto & gift_next = g_data[r[i+1]];

        w -= gift.Weight();

        res += w*Dist(gift.Loc(), gift_next.Loc());
        ++test_counter;
    }

    w = sleigh_base_weight;
    res += w*Dist(north_pole, g_data[r[r.size()-1]].Loc() );

    return res;
}

void Graph::sg_knn()
{

	int i,j,l, k;
	k = (int) par;
	std::vector<int> *node;

	if(prepR==0)// if not preprocessed
	{
		if(dbg) Rprintf("%i-nn: ", k);
		int n = pp->size();
		double *dists2_i, *dists2_i2;
		dists2_i = new double[n];
		dists2_i2 = new double[n];
		for(i=0;i<pp->size();i++) //for each point
		{
			for(j=0;j<pp->size();j++) dists2_i2[j]=dists2_i[j]= Dist(&i,&j); //gather the distances to others
			qsort( dists2_i, pp->size(), sizeof(double), compare_doubles); // sort distances, rising
			for(j=1;j <= k;j++) // find the k nearest
				for(l=0;l<pp->size();l++)
					if( dists2_i[j] == dists2_i2[l] ) //with distance comparison
					{
						nodelist[i].push_back(l+1);
						break;
					}

		}
	}
	else{ //preprocessed
		if(dbg) Rprintf("%i-nn (shrinking):", k);
		double *dists2_i, *dists2_i2;
		for(i=0;i<pp->size();i++) //for each point
		{
			node = new std::vector<int>;
			dists2_i = new double [nodelist[i].size()];
			dists2_i2 = new double [nodelist[i].size()];
			if((int)nodelist[i].size()<k){ Rprintf("\n preprocessing R too small, not enough neighbours (point #%i)!!\n",i+1); return;}
			for(l=	0;l< (int)nodelist[i].size();l++)
			{
				j = nodelist[i][l]-1;
				dists2_i2[l]=Dist(&i,&j); //gather the distances to others, given preprocessing
				dists2_i[l]=dists2_i2[l];
			}
			qsort( dists2_i, nodelist[i].size() , sizeof(double),compare_doubles); // sort distances, rising
			for(j=0;j<k;j++) // find the k nearest
				for(l=0;l<(int)nodelist[i].size();l++)
					if( dists2_i[j] == dists2_i2[l] ) //with distance comparison
					{
						node->push_back(nodelist[i][l]);
						break;
					}
			nodelist[i].clear();nodelist[i].resize(0);
			for(j=0;j < (int)node->size();j++) nodelist[i].push_back( (*node)[j] );
			delete node;
			delete[] dists2_i;
			delete[] dists2_i2;
		}
	}
	 if(dbg) Rprintf(" Ok.");
}

double Adjust(){
  int i = 0;
  while (i < n && s[i] == 'T')
    ++i;
  if (i == n)
    return -0.5;
  if (i == 0){
    if (s[i] == 'S')
      return 0;
    else
      return -2 + M_PI / 2;
  }
  double x1 = 0.5;
  double y1 = sqrt(3) / 2;
  if (s[i] == 'S'){
    double x2 = i;
    double y2 = 1.0;
    return -x2 - 1 + Dist(0, 0, x1, y1) + Dist(x1, y1, x2, y2); 
  }
  else{
    double xc    = i + 0.5;
    double yc    = 0.5;
    double h     = Dist(x1, y1, xc, yc);
    double alpha = atan2(y1 - yc, x1 - xc);
    double fi    = acos(0.5 / h);
    double x2    = xc + 0.5 * cos(alpha - fi);
    double y2    = yc + 0.5 * sin(alpha - fi);
    return -xc - 1 + Dist(0, 0, x1, y1) + Dist(x1, y1, x2, y2) + 0.5 * (alpha - fi - M_PI / 2);
  }
}

void FillRandomImg(CSimpleImage& Image, int ImageNo)
{
   static bool Initialized = false;
   const static int NbImages = 2;
   static CSimpleImage Images[NbImages];
   static CImage<float> FloatImages[NbImages];

   //assert(ImageNo >= 0 && ImageNo < NbImages);

   if (!Initialized)
   {
      // Random image generation is time consuming
      // So we do it only once

      std::mt19937 Rand;   // Mersenne twister pseudo random number generator

      // Uniform distribution that can set all but the leftmost bit
      std::uniform_int_distribution<int> Dist(0, 0xFF);

      SSize Big = GetBiggestImage();

      for (int i = 0; i < NbImages; i++)
      {
         Images[i].Create(Big.Width, Big.Height, 1, SImage::S32);
         uint uByteWidth = Images[i].BytesWidth();
         for (uint y = 0; y < Images[i].Height; y++)
            for (uint b = 0; b < uByteWidth; b++)
               Images[i].Data(y)[b] = Dist(Rand);
      }


      std::normal_distribution<float> FloatDist(0, 1);

      for (int i = 0; i < NbImages; i++)
      {
         FloatImages[i].Create(Big.Width, Big.Height, 1, SImage::F32);
         for (uint y = 0; y < FloatImages[i].Height; y++)
            for (uint x = 0; x < FloatImages[i].Width; x++)
               FloatImages[i](x, y) = FloatDist(Rand);
      }

      Initialized = true;
   }

   uint uByteWidth = Image.BytesWidth();
   
   if (Image.Type == Image.F32)
   {
      for (uint y = 0; y < Image.Height; y++)
         memcpy(Image.Data(y), FloatImages[ImageNo].Data(y), uByteWidth);

      return;
   }

   for (uint y = 0; y < Image.Height; y++)
      memcpy(Image.Data(y), Images[ImageNo].Data(y), uByteWidth);
}

void  cDistorBilin::Diff(ElMatrix<REAL> & aM,Pt2dr aP) const
{
    // InitEtatFromCorner(ToCoordGrid(aP));

    Pt2dr aPds;
    GetParamCorner(mCurCorner,aPds,ToCoordGrid(aP)); 
    const Pt2dr & aP00 =  Dist(mCurCorner+CamBilinCorn[0] ) ;
    const Pt2dr & aP10 =  Dist(mCurCorner+CamBilinCorn[1] ) ;
    const Pt2dr & aP01 =  Dist(mCurCorner+CamBilinCorn[2] ) ;
    const Pt2dr & aP11 =  Dist(mCurCorner+CamBilinCorn[3] ) ;


    Pt2dr aGx =    ((aP10-aP00)*aPds.y + (aP11-aP01)*(1-aPds.y))  / mStep.x;
    Pt2dr aGy =    ((aP01-aP00)*aPds.x + (aP11-aP10)*(1-aPds.x))  / mStep.y;

    aM.ResizeInside(2,2);
    SetCol(aM,0,aGx);
    SetCol(aM,1,aGy);

    // A conserver, verification par diff std
    if (0)
    {
        ElMatrix<REAL> aM2(2,2);
        DiffByDiffFinies(aM2,aP,euclid(mStep)/1e4);
        static double aDMax = 0;
        double aD = aM.L2(aM2);
        if (aD>aDMax)
        {
            aDMax = aD;
            std::cout << "DDDD " << aD << "\n";
        }
    }
    // InitEtatFromCorner(ToCoordGrid(aP));

}

void * LogMem::alloc(ulen len)
 {
  if( !TryAlign(len) ) return 0;
  
  if( ptr<lim )
    {
     ulen s=Dist(lim,mem_lim);
     
     if( len<=s )
       {
        void *ret=lim;
        
        lim+=len;
        
        return ret;
       }
       
     s=Dist(mem,ptr);  
     
     if( len<s )
       {
        cut=lim;
        lim=mem+len;
        
        return mem;
       }
       
     return 0;  
    }
    
  if( ptr>lim )  
    {
     ulen s=Dist(lim,ptr);
     
     if( len<s )
       {
        void *ret=lim;
        
        lim+=len;
        
        return ret;
       }
       
     return 0;  
    }
    
  ulen s=Dist(lim,mem_lim);
     
  if( len<s )
    {
     void *ret=lim;
        
     lim+=len;
        
     return ret;
    }
       
  return 0;  
 }

Pt2dr cDistorBilin::Direct(Pt2dr aP) const
{
    InitEtatFromCorner(ToCoordGrid(aP));

    return   
             Dist(mCurCorner +CamBilinCorn[0] ) * mPds[0]
           + Dist(mCurCorner +CamBilinCorn[1] ) * mPds[1]
           + Dist(mCurCorner +CamBilinCorn[2] ) * mPds[2]
           + Dist(mCurCorner +CamBilinCorn[3] ) * mPds[3];
}

void CBasePoly::Flip()
{
	for(uint32 i=0; i < m_Indices/2; i++ )
	{
		uint32 temp = m_Indices[i];
		m_Indices[i] = m_Indices[m_Indices-i-1];
		m_Indices[m_Indices-i-1] = temp;
	}

	Normal() = -Normal();
	Dist() = -Dist();
}

void cDistorBilin::V_SetScalingTranslate(const double & aF,const Pt2dr & aPP)
{
   for (int aKY=0 ; aKY<= mNb.y ; aKY++)
   {
       for (int aKX=0 ; aKX<= mNb.x ; aKX++)
       {
           Dist(Pt2di(aKX,aKY)) = ( Dist(Pt2di(aKX,aKY))- aPP) / aF;
       }
   }
   mP0 = (mP0-aPP) / aF;
   mP1 = (mP1-aPP) / aF;
   mStep = mStep / aF;
}

void CEditPoly::Flip()
{
	uint32		i, temp;

	for( i=0; i < m_Indices/2; i++ )
	{
		temp = m_Indices[i];
		m_Indices[i] = m_Indices[m_Indices-i-1];
		m_Indices[m_Indices-i-1] = temp;
	}

	Normal() = -Normal();
	Dist() = -Dist();
}

//extern int spp_send(char* buf, int count);
static uint16_t gesture_caculate(int index, const int8_t* point)
{
  const int8_t *currentGesture = GestureData[index + gestureoffset];
  uint8_t gestureLength = GestureDataLength[index + gestureoffset];
  uint16_t *distance = distances + (MAX_GESTURES * index);
  // caculate with template
  uint16_t lastvalue = Dist(&currentGesture[0], point);
  
  if (count == MOVE_WINDOW)
  {
    distance[0] = lastvalue;
    for(int tindex = 1; tindex < gestureLength; tindex++)
    {
      distance[tindex] = distance[tindex - 1] + Dist(&currentGesture[tindex * 3], point);
    }
  }
  else
  {
    if (lastvalue > distance[0])
      lastvalue = distance[0];
    for(int tindex = 1; tindex < gestureLength; tindex++)
    {
      uint16_t local = Dist(&currentGesture[tindex * 3], point);
      uint16_t min = lastvalue;
      if (min > distance[tindex])
        min = distance[tindex];
      if (min > distance[tindex - 1])
        min = distance[tindex - 1];
      if (min > lastvalue)
        min = lastvalue;
      distance[tindex - 1] = lastvalue;
      lastvalue = local + min;
    }
    distance[gestureLength - 1] = lastvalue;
  }
#if 0
  for(int i = 0; i < gestureLength; i++)
  {
    PRINTF("%d ",distance[i]);
  }
  PRINTF("\n  ");
  for(int i = 1; i < gestureLength; i++)
  {
    PRINTF("%d ",distance[i] - distance[i-1]);
  }
  PRINTF("\n");
#endif
  return distance[gestureLength - 1]/(gestureLength + count/MOVE_STEP - 1);
}

bool CBasePoly::LoadBasePolyTBW( CAbstractIO& InFile )
{
	//load up all the vertices
	uint32 nNumVerts;
	InFile >> nNumVerts;

	m_Indices.SetSize( nNumVerts );

	for(uint32 nCurrVert = 0; nCurrVert < nNumVerts; nCurrVert++)
	{
		uint32 nIndex;
		InFile >> nIndex;

		Index(nCurrVert) = nIndex;
	}

	//now read in the plane data
	InFile >> Normal().x;
	InFile >> Normal().y;
	InFile >> Normal().z;

	InFile >> Dist();

	return true;
}

 void PathPlanner::disconnectNodes(Node * path) {
     SearchSpaceNode * S;
     Node * temp;
     double distance;
     if (!path)
         return;
     S = searchspace;
     int numConnections = this->MAXNODES;
     while (S != NULL) {
         temp = path;
         while (temp != NULL) {
             distance = Dist(S->location, temp->pose.p);
             if (distance == 0)//S == temp->pose.p.position.x)// && distance !=0)
             {
                 S->children.pop_back();
                 bool t = removeNode(S->location);
                 numConnections--;
                 delete S;
             }
             temp = temp->next;
         }
         S = S->next;
     }
     searchspace = S;
     std::cout << "\n	--->>> NODES CONNECTED <<<---	" << numConnections;
     this->MAXNODES = numConnections; //searchspace->id;
 }

void cEqHomogFormelle::StdRepondere(ElPackHomologue & aPack,REAL aCoeff)
{
    cElHomographie H1 = HF1().HomCur();
    cElHomographie H2 = HF2().HomCur();
    ElDistRadiale_PolynImpair Dist(1e5,Pt2dr(0,0));
    if (DRF()) 
       Dist = DRF()->DistCur();

    for
    (
         ElPackHomologue::iterator it=aPack.begin();
         it!=aPack.end();
         it++
    )
    {
          Pt2dr aP1 = it->P1();
          Pt2dr aP2 = it->P2();
          if (mDRF)
          {
              aP1 = Dist.Direct(aP1);
              aP2 = Dist.Direct(aP2);
          }
          Pt2dr Ec = H1.Direct(Dist.Direct(it->P1())) - H2.Direct(Dist.Direct(it->P2()));

          REAL D = euclid(Ec);
         
          REAL Pds = 1 / (1 + ElSquare(D/aCoeff));
          it->Pds() = Pds;
    }
}

GeomPerfectMatching::REAL GeomPerfectMatching::SolveComplete()
{
	if (node_num != node_num_max) { printf("ComputeCost() cannot be called before all points have been added!\n"); exit(1); }

	PointId p, q;
	int e = 0, E = node_num*(node_num-1)/2;
	PerfectMatching* pm = new PerfectMatching(node_num, E);
	for (p=0; p<node_num; p++)
	{
		for (q=p+1; q<node_num; q++)
		{
			pm->AddEdge(p, q, Dist(p, q));
		}
	}
	pm->options = options;
	pm->Solve();
	for (p=0; p<node_num; p++)
	{
		for (q=p+1; q<node_num; q++)
		{
			if (pm->GetSolution(e++))
			{
				matching[p] = q;
				matching[q] = p;
			}
		}
	}
	delete pm;
	return ComputeCost(matching);
}