Пример #1
0
tuple 
PyOctreePointCloudSearch::radiusSearch(tuple point, double radius, unsigned int max_nn)
{
  std::vector<int> k_indices;
  std::vector<float> k_sqr_distances;
  if (max_nn > 0)
  {
    k_indices.resize(max_nn);
    k_sqr_distances.resize(max_nn);
  }

  pxyz p;
  p.x = extract<float>(point[0]);
  p.y = extract<float>(point[1]);
  p.z = extract<float>(point[2]);
  boost::shared_ptr<pcl::octree::OctreePointCloudSearch<pxyz>> searchPtr = boost::dynamic_pointer_cast<pcl::octree::OctreePointCloudSearch<pxyz>>(this->instance_);
  npy_intp k = searchPtr->radiusSearch(p, radius, k_indices, k_sqr_distances, max_nn);

  numeric::array sqdist(handle<>(PyArray_SimpleNew(1, &k, NPY_FLOAT32)));
  numeric::array ind(handle<>(PyArray_SimpleNew(1, &k, NPY_INT32)));

  for (int i = 0; i < k; i++)
  {
    sqdist[i] = k_sqr_distances[i];
    ind[i] = k_indices[i];
  }

  return make_tuple(ind, sqdist);
}
Пример #2
0
int main (void){
	int cnum, couche, cplr, i, j, col_num, x, y, ang, dist, thrown[2];
	double sin_table[361], cos_table[361], min_dist;
	double conv = acos(-1)/180;
	bool dbg = 0;
	for(i = 0; i < 361; i++) sin_table[i] = sin(i*conv), cos_table[i] = cos(i*conv);
	scanf("%d",&cnum);
	while(cnum--){
		cplr = couche = thrown[0] = thrown[1] = 0;
		scanf("%s%s",n[0],n[1]);
		for(i = 0; i < 7; i++){
			if(i && ++thrown[cplr] > 3) cplr = 1-cplr;
			scanf("%d%d%d%d",&x,&y,&ang,&dist);
			start = Ball(x,y,cplr,0);
			b[i] = Ball(x+cos_table[ang]*dist,y+sin_table[ang]*dist,cplr,i);
			for(col_num = j = 0; j < i; j++)
				if(colinear(start,b[j],b[i]))
					col[col_num++] = Ball(b[j].x,b[j].y,b[j].plr,b[j].n);
			std::sort(col,col+col_num,closerToStart);
			for(j = 0; j < col_num; j++){
				int aux = col[j].plr;
				b[col[j].n].plr = b[i].plr;
				b[i].plr = aux;
			}
			min_dist = 1<<23;
			for(j = 0; j <= i; j++)
				if(b[j].plr == 2){ couche = j; break;}
			for(j = 0; j <= i; j++){
				if(j == couche) continue;
				double temp = sqdist(b[j],b[couche]);
				if(temp < min_dist){
					min_dist = temp;
					cplr = 1 - b[j].plr;
				}
			}
			if(i == 0) b[i].plr = 2;
		}
		for(i = 0; i < 7; i++)
			if(b[i].plr == 2){
				start = Ball(b[i].x,b[i].y,2,0);
				break;
			}
		std::sort(b,b+7,closerToStart);
		if(b[1].plr == 2) std::swap(b[0],b[1]);
		for(i = 2; b[i].plr == b[1].plr; i++);
		printf("%s %d\n",n[b[1].plr],i-1);
	}
	return 0;
}
Пример #3
0
void SiteMan::get_dist_mat(int dist_max, int dist_map[1000][1000]) {
	//int * dist_map = new int[dist_max][dist_max];
	Site* s1;
	Site* s2;
	for (uint i = 0; i < this->sites->size(); ++i) {	
		for (uint j = 0; j < this->sites->size(); ++j)	{
			if (i == j) {
				dist_map[i][j] = -1;
			}
			else {
				s1 = get(i);
				s2 = get(j);

				dist_map[i][j] = sqdist(s1->pos_x, s1->pos_y, s2->pos_x, s2->pos_y);
				// if (sqdist(s1->pos_x, s1->pos_y, s2->pos_x, s2->pos_y) <= square(dist_max)) {
				// 	dist_map[i][j] = 1;
				// }
				// else {
				// 	dist_map[i][j] = 0;
				// }
			}
		}
	}
}
Пример #4
0
	void search(D &d, typename D::State &s0) {
		this->start();

		Node *root = pool.construct();
		root->parent = NULL;
		d.pack(root->state, s0);

		double pt[K];
		s0.vector(&d, pt);
		for (unsigned int i = 0; i < K; i++)
			root->point[i] = pt[i];
		tree.insert(pt, root);

		for ( ; ; ) {
			sample(pt);
			auto near = tree.nearest(pt);
			Node *node = near->data;
			State buf, &state = d.unpack(buf, node->state);
				
			SearchAlgorithm<D>::res.expd++;
	
			Node *kid = pool.construct();
			kid->parent = node;
			double bestpt[K];
			double dist = std::numeric_limits<double>::infinity();

			typename D::Operators ops(d, state);
			for (unsigned int i = 0; i < ops.size(); i++) {
				SearchAlgorithm<D>::res.gend++;
				if (node->ops.find(ops[i]) != node->ops.end())
					continue;

				Edge e(d, state, ops[i]);

				if (d.isgoal(e.state)) {
					d.pack(kid->state, e.state);
					kid->op = ops[i];
					solpath<D, Node>(d, kid, this->res);
					goto done;
				}

				double kidpt[K];
				e.state.vector(&d, kidpt);
				
				double dst = sqdist(pt, kidpt);
				if (dst < dist) {
					dist = dst;
					for (unsigned int j = 0; j < K; j++)
						bestpt[j] = kidpt[j];
					d.pack(kid->state, e.state);
					kid->op = ops[i];
				}
			}
			if (std::isinf(dist)) {
				pool.destruct(kid);
				continue;
			}
	
			node->ops.insert(kid->op);
			tree.insert(bestpt, kid);
			for (unsigned int i = 0; i < K; i++)
				kid->point[i] = bestpt[i];
		}

done:

		this->finish();
		drawtree(d);
	}
Пример #5
0
void clustknb_new_w(double *XData, unsigned int k, double *Weights,
                    unsigned int use_old_mus, double *mus_start, double *mus, 
                    double *R, unsigned int XSize, unsigned int XDim, 
		    unsigned int disp) 
{
    unsigned int i, changed=1 ;
    const unsigned int XDimk=XDim*k ;
    unsigned int Iter=0 ;

    unsigned int *ClList = (unsigned int*) calloc(XSize, sizeof(unsigned int)) ;
    double *SetWeigths = (double*) calloc(k, sizeof(double)) ;
    double *oldmus = (double*) calloc(XDimk, sizeof(double)) ;
    double *dists = (double*) calloc(k*XSize, sizeof(double)) ;
    double norm_oldmus_mus ;

    /* ClList=zeros(XSize,1) ; */
    for (i=0; i<XSize; i++) ClList[i]=0 ;
    /* SetWeigths=zeros(k,1) ; */
    for (i=0; i<k; i++) SetWeigths[i]=0 ;
    
    /* mus=zeros(XDim, k) ; */
    for (i=0; i<XDimk; i++) mus[i]=0 ;

    if (!use_old_mus)
    {
        /* random clustering (select random subsets) */
        /*  ks=ceil(rand(1,XSize)*k) ;
	 *  for i=1:k,
	 *	actks= (ks==i) ;
	 *	c=sum(actks) ;
	 *	SetWeigths(i)=c ;
	 *
	 *	ClList(actks)=i*ones(1, c) ;
         *
	 *	if ~mus_recalc,
	 *		if c>1
	 *			mus(:,i) = sum(XData(:,actks)')'/c ;
	 *		elseif c>0
	 *			mus(:,i) = XData(:,actks) ;
	 *		end ;
	 *	end ;
	 *   end ; */

        for (i=0; i<XSize; i++) 
        {
            const unsigned int Cl= (int) ( rand()%k ) ;
            unsigned int j ;
	    double weight=Weights[i] ;
            
            SetWeigths[Cl]+=weight ;
            ClList[i]=Cl ;

            for (j=0; j<XDim; j++)
              mus[Cl*XDim+j] += weight*XData[i*XDim+j] ;
        } ;
        for (i=0; i<k; i++)
        {
            unsigned int j ;
            
            if (SetWeigths[i]!=0.0)
              for (j=0; j<XDim; j++)
                mus[i*XDim+j] /= SetWeigths[i] ;
        } ;
    }
    else 
    {
        assert(mus_start!=NULL) ;

        sqdist(mus_start, XData, dists, k, XSize, XDim) ;

        for (i=0; i<XSize; i++)
        {
            double mini=dists[i*k] ;
            unsigned int Cl = 0, j ;
            
            for (j=1; j<k; j++)
              if (dists[i*k+j]<mini)
              {
                Cl=j ;
                mini=dists[i*k+j] ;
              } ;
            ClList[i]=Cl ;
        } ;
    
        /* Compute the sum of all points belonging to a cluster 
         * and count the points */
        for (i=0; i<XSize; i++) 
        {
            const unsigned int Cl = ClList[i] ;
            unsigned int j ;
	    double weight=Weights[i] ;
            SetWeigths[Cl]+=weight ;
#ifndef MUSRECALC
            for (j=0; j<XDim; j++)
              mus[Cl*XDim+j] += weight*XData[i*XDim+j] ;
#endif
        } ;
#ifndef MUSRECALC
        /* normalization to get the mean */ 
        for (i=0; i<k; i++)
            if (SetWeigths[i]!=0.0)
            {
	      unsigned int j ;
	      for (j=0; j<XDim; j++)
                  mus[i*XDim+j] /= SetWeigths[i] ;
	    } ;
#endif
    } ;

    for (i=0; i<XDimk; i++) oldmus[i]=-1 ;

    while (changed && (Iter<10000))
    {
        Iter++ ;
	if (Iter==9999)
	  printf("Oops\n") ;
        if (disp)
	  printf("Assignment of %i patterns changed.\n", changed) ;
        changed=0 ;

#ifdef MUSRECALC
	/* mus=zeros(XDim, k) ; */
	for (i=0; i<XDimk; i++) mus[i]=0 ;

        for (i=0; i<XSize; i++) 
        {
            unsigned int j ;
            unsigned int Cl=ClList[i] ;
	    double weight=Weights[i] ;

            for (j=0; j<XDim; j++)
              mus[Cl*XDim+j] += weight*XData[i*XDim+j] ;
        } ;
        for (i=0; i<k; i++)
        {
            unsigned int j ;
            
            if (SetWeigths[i]!=0.0)
              for (j=0; j<XDim; j++)
                mus[i*XDim+j] /= SetWeigths[i] ;
        } ;
#endif

        for (i=0; i<XSize; i++)
        {
	    /* ks=ceil(rand(1,XSize)*XSize) ; */
	    const unsigned int Pat=(int) rand()%XSize ;
            const unsigned int ClList_Pat=ClList[Pat], Pat_XDim=Pat*XDim ;
            unsigned int imini, j ;
            double mini, weight ;
    
	    weight=Weights[Pat] ;

            /* compute the distance of this point to all centers */
            /* dists=sqdist(mus',XData) ; */
            sqdist(mus, &XData[Pat*XDim], dists, k, 1, XDim) ;

	    /* [mini,imini]=min(dists(:,i)) ; */
            imini=0 ; mini=dists[0] ;
            for (j=1; j<k; j++)
              if (dists[j]<mini)
              {
                mini=dists[j] ;
                imini=j ;
              } ;
        
            if (imini!=ClList_Pat)
            {
              unsigned int j ;

	      changed= changed + 1 ;

              /* SetWeigths(imini) = SetWeigths(imini) + weight ; */
	      SetWeigths[imini]+= weight ;
              /* SetWeigths(j)     = SetWeigths(j)     - weight ; */
	      SetWeigths[ClList_Pat]-= weight ;

              /* mu_new=mu_old + (x - mu_old)/(n+1) */
	      /* mus(:,imini)=mus(:,imini) + (XData(:,i) - mus(:,imini)) * (weight / SetWeigths(imini)) ; */
              for (j=0; j<XDim; j++)
                 mus[imini*XDim+j] += (XData[Pat*XDim+j] - mus[imini*XDim+j]) *(weight / SetWeigths[imini]) ;

              /* mu_new = mu_old - (x - mu_old)/(n-1) */
              /* if SetWeigths(j)~=0 */
	      if (SetWeigths[ClList_Pat]!=0.0)
	          /* mus(:,j)=mus(:,j) - (XData(:,i) - mus(:,j)) * (weight/SetWeigths(j)) ; */
		  for (j=0; j<XDim; j++)
		      mus[ClList_Pat*XDim+j]-=(XData[Pat*XDim+j]-mus[ClList_Pat*XDim+j])*(weight/SetWeigths[ClList_Pat]);
              else
	          /*  mus(:,j)=zeros(XDim,1) ; */
                  for (j=0; j<XDim; j++)
                      mus[ClList_Pat*XDim+j]=0 ;

	      /* ClList(i)= imini ; */
              ClList[Pat] = imini ;
            } ;
        } ;
    } ;

    /* compute the ,,variances'' of the clusters */
    for (i=0; i<k; i++)
    {
        double rmin1, rmin2 ;
        unsigned int j ;
	rmin1=BIG ;
	rmin2=BIG ;

        for (j=0; j<k; j++) 
        {
            if (j!=i)
            {
                unsigned int l ;
                double dist = 0 ;

                for (l=0; l<XDim; l++)
                  dist+=sqr(mus[i*XDim+l]-mus[j*XDim+l]) ;
		/*		printf("%f,", sqrt(dist)) ;*/
                if ((dist<rmin2) && (dist>=rmin1))
		  rmin2=dist ;
                if (dist<rmin1) 
		{
		  rmin2=rmin1 ;
		  rmin1=dist ;
		} ;
            } ;
	} ;
        R[i]=(0.7*sqrt(rmin1)+0.3*sqrt(rmin2)) ;
	/*	printf("\n(%f, %f->%f)", sqrt(rmin1), sqrt(rmin2), R[i]) ;*/
    } ;
 
    free(ClList) ;
    free(SetWeigths) ;
    free(oldmus) ;
    free(dists) ;
} 
Пример #6
0
void TeamDetector::findRobotsByTeamMarkerOnly(::google::protobuf::RepeatedPtrField< ::SSL_DetectionRobot >* robots, int team_color_id, const Image<raw8> * image, CMVision::ColorRegionList * colorlist)
{
    filter_team.init( colorlist->getRegionList(team_color_id).getInitialElement() );

    //TODO: change these to update on demand:
    //local variables
    const CMVision::Region * reg=0;
    SSL_DetectionRobot * robot=0;
    while((reg = filter_team.getNext()) != 0) {
        vector2d reg_img_center(reg->cen_x,reg->cen_y);
        vector3d reg_center3d;
        _camera_params.image2field(reg_center3d,reg_img_center,_robot_height);
        vector2d reg_center(reg_center3d.x,reg_center3d.y);

        //TODO: add confidence masking:
        //float conf = det.mask.get(reg->cen_x,reg->cen_y);
        double conf=1.0;
        if (field_filter.isInFieldOrPlayableBoundary(reg_center) &&  ((_histogram_enable==false) || checkHistogram(reg,image)==true)) {
            double area = getRegionArea(reg,_robot_height);
            double area_err = fabs(area - _center_marker_area_mean);

            conf *= GaussianVsUniform(area_err, sq(_center_marker_area_stddev), _center_marker_uniform);
            //printf("-------------\n");
            if (conf < 1e-6) conf=0.0;

            /*if(unlikely(verbose > 2)){
              printf("    area=%0.1f err=%4.1f conf=%0.6f\n",area,area_err,conf);
            }
            if(det.debug) det.color(reg,rc,conf);*/

            //allow twice as many robots for now...
            //duplicate filtering will take care of the rest below:
            robot=addRobot(robots,conf,_max_robots*2);

            if (robot!=0) {
                //setup robot:
                robot->set_x(reg_center.x);
                robot->set_y(reg_center.y);
                robot->set_pixel_x(reg->cen_x);
                robot->set_pixel_y(reg->cen_y);
                robot->set_height(_robot_height);
            }
        }
    }

    // remove duplicates ... keep the ones with higher confidence:
    int size=robots->size();
    for(int i=0; i<size; i++) {
        for(int j=i+1; j<size; j++) {
            if(sqdist(vector2d(robots->Get(i).x(),robots->Get(i).y()),vector2d(robots->Get(j).x(),robots->Get(j).y())) < sq(_center_marker_duplicate_distance)) {
                robots->Mutable(i)->set_confidence(0.0);
            }
        }
    }

    //remove items with 0-confidence:
    stripRobots(robots);

    //remove extra items:
    while(robots->size() > _max_robots) {
        robots->RemoveLast();
    }

}
Пример #7
0
void *compute_3d_power_edges(simplex *s, void *p) { 

	static out_func *out_func_here;
	point v[MAXDIM];
	int j, k, inf=0, numedges, ns, l, nedge0, nedge1, nremv, nnextv, l1, l2, nk;
	site edge0, edge1, nextv, remv, prevv;
    double ta[4][4], r1, r2, d, e;
	simplex *prevs, *nexts;
        
	if (p) {out_func_here = (out_func*)p; if (!s) return NULL;}

	
	if ((s->status == CNV)||(s->status == SLV)) return NULL; /* skip inf faces */
	for (j=0;j<cdim;j++) {
        v[j] = s->neigh[j].vert; 
        for (k=0;k<4;k++) {
            ta[j][k] = v[j][k]/mult_up; /* restore original coords   */
        }
	}
 
	if (!inf) {
        for (k=0;k<6;k++) { /* for each edge */
            if (s->edgestatus[k]==FIRST_EDGE) { /* not visited edge */
	       
                /* check the dihedral angle */
                d = sqdist(ta[v1[k]],ta[v2[k]]);
                r1 = SQ(ta[v1[k]][0])+SQ(ta[v1[k]][1])+
                    SQ(ta[v1[k]][2])-ta[v1[k]][3];
                r2 = SQ(ta[v2[k]][0])+SQ(ta[v2[k]][1])+
                    SQ(ta[v2[k]][2])-ta[v2[k]][3];
                e = 2 * sqrt(r1) * sqrt(r2);
                if ((d >= (r1+r2+e)) || ((d-r1-r2)/e > theta )) {
                    /* fprintf(DFILE,"%f\n",(d-r1-r2)/e);*/
                    /* edge0, edge1 are the vertices of the edge */
                    edge0 = s->neigh[v1[k]].vert;
                    edge1 = s->neigh[v2[k]].vert;
                    nextv = s->neigh[v3[k]].vert;
                    /* nextv is the opposite vtx of the next simplex */
                    remv = s->neigh[v4[k]].vert;
                    /* remv is a vtx of the next simplex with edge0, edge1 */
                    prevv = remv;
                    /* prevv is the vtx shared by prevs and nexts besides edge0, edge1 */

                    /* construct its dual power face */
                    s->edgestatus[k]=POW;
		  
                    /* visit the next simplex */
                    /* print orthocenter of s->neigh[v3[k]].simp ...*/
                    prevs = s;
                    nexts = s->neigh[v3[k]].simp;
		  
                    ns = v3[k];
                    numedges=0;
                    while (nexts != s) {
                        if (nexts->status == CNV) {
                            fprintf(DFILE,"inf reg face\n");
                            break;
                        }
                        else {
                            fprintf(PC,"%f %f %f\n",prevs->vv[0],prevs->vv[1],prevs->vv[2]);
                            numedges++;numvtxs++;
                            /* find edgenumber k of nexts for this edge */
                            for (l=0;l<4;l++) {
                                if (nexts->neigh[l].vert==edge0) {
                                    /* l == v1[k] */
                                    nedge0 = l;continue;
                                }
                                else if (nexts->neigh[l].vert==edge1) {
                                    /* l == v2[k] */
                                    nedge1 = l;continue;
                                }
                                else if (nexts->neigh[l].vert==prevv) {
                                    nremv = l;continue;
                                }
                                else if (nexts->neigh[l].vert==nextv) {
                                    nnextv = l;
                                    continue; 
			 
                                }
                                else {
                                    nnextv = l;
                                }
                            }
		      
                            if (nedge0 > nedge1) { l1 = nedge1; l2 = nedge0; }
                            else { l2 = nedge1; l1 = nedge0; }
                            if (l1==0) {
                                if (l2==1) nk = 0;
                                else if (l2==2) nk = 1;
                                else nk = 2;
                            }
                            else if (l1==1) {
                                if (l2==2) nk = 3;
                                else nk = 4;
                            }
                            else nk = 5;  
                            /* found nk for the edge */
                            nexts->edgestatus[nk]=POW; /* record that it's visited */
                            /* visit next simplex (opposite vertex ns )*/
                            prevs = nexts;
                            prevv = nexts->neigh[nnextv].vert;
                            nexts = nexts->neigh[nremv].simp;
                        }
                    }
                    fprintf(PC,"%f %f %f\n", prevs->vv[0], prevs->vv[1], prevs->vv[2]);
                    numedges++;numvtxs++;
                    fprintf(PNF,"%d ",numedges);
                    for (l=numedges;l>0;l--) {
                        fprintf(PNF, "%d ",numvtxs-l);
                    }
                    fprintf(PNF,"\n");numfaces++;
                }
                else { 
                    s->edgestatus[k]=NOT_POW;
                }
            }	      /* skip if the edge is visited before */
        }
	}
	/* ignore inf faces */

	return NULL;

}
Пример #8
0
void *compute_3d_power_vv(simplex *s, void *p) { 

	static out_func *out_func_here;
	point v[MAXDIM];
	int j,k,inf=0, index, visited_edge;
    double cc[3], cond, ta[4][4], d, r1, r2, e;
	struct edgesimp *newplist, *pindex;

	if (p) {
        out_func_here = (out_func*)p; 
        if (!s) return NULL;
	}

	index = 0;
	for (j=0;j<cdim;j++) {
        v[j] = s->neigh[j].vert; 
        /* v[j] stores coordinates of j'th vertex of simplex s; j=0..3 */ 
        if (v[j]==infinity) { /* means simplex s is on the convex hull */
            inf=1;
            continue; /* skip the rest of the for loop; process next vertex */
        }  
        /*i=(site_num)(v[j]);  i is the index of the vertex v[j] */
        for (k=0;k<4;k++) {
            ta[index][k] = v[j][k]/mult_up; /* restore original coords   */
            /* if inf=1, ta[0],ta[1],ta[2] are non-infinite vertices of s*/
            /*    inf=0, ta[0],ta[1],ta[2],ta[3] are 4 vertices of s     */
        }
        index++;
	}
 
	/* if not faces on convex hull, process */
	if (!inf) { 

        /* build structure for each edge, including angle of intersection */
        for (k=0;k<6;k++) { 
            if (s->edgestatus[k]==FIRST_EDGE) { /* not visited edge */
                pindex = adjlist[site_numm(v[v1[k]])].eptr;
                visited_edge = 0;
                while (pindex!= NULL) {
                    if (pindex->pid == site_numm(v[v2[k]])) { /* already in the list */
                        visited_edge = 1;
                        break;
                    }
                    pindex = pindex->next;
                }
	
                if (!visited_edge) {
                    d = sqdist(ta[v1[k]],ta[v2[k]]);
                    r1 = SQ(ta[v1[k]][0])+SQ(ta[v1[k]][1])+SQ(ta[v1[k]][2])-ta[v1[k]][3];
                    r2 = SQ(ta[v2[k]][0])+SQ(ta[v2[k]][1])+SQ(ta[v2[k]][2])-ta[v2[k]][3];
                    e = 2 * sqrt(r1) * sqrt(r2);

                    newplist = (struct edgesimp *) malloc(sizeof(struct edgesimp));
                    newplist->simp = s;
                    newplist->kth = k;
                    newplist->angle = (r1+r2-d)/e;
                    newplist->pid = site_numm(v[v1[k]]);
                    newplist->next = adjlist[site_numm(v[v2[k]])].eptr;
                    adjlist[site_numm(v[v2[k]])].eptr = newplist;

                    newplist = (struct edgesimp *) malloc(sizeof(struct edgesimp));
                    newplist->simp = s;
                    newplist->kth = k;
                    newplist->angle = (r1+r2-d)/e;
                    newplist->pid = site_numm(v[v2[k]]);
                    newplist->next = adjlist[site_numm(v[v1[k]])].eptr;
                    adjlist[site_numm(v[v1[k]])].eptr = newplist;

                    s->edgestatus[k] = VISITED;
                }
            }
        }

        tetorthocenter(ta[0], ta[1], ta[2], ta[3], cc, &cond);	 
        /* cc is the displacement of orthocenter from ta[0] */
        /* cond is the denominator ( orient2d ) value        */
        if (cond!=0) { /* ignore them if cond = 0 */
			s->vv = (Coord*) malloc(sizeof(Coord)*3);
			for (k=0;k<3;k++) {
                s->vv[k] = ta[0][k]+cc[k];	
			}
			s->status = VV;			
			
        }
        else { /* if cond=0, s is SLIVER */
            fprintf(DFILE,"sliver!\n");
            s->vv = NULL;
            s->status = SLV;
        }
	}
	else { /* if on conv hull, ignore */
		s->vv = NULL;
		s->status = CNV;
	} 	

	return NULL;
}
Пример #9
0
bool closerToStart(Ball a, Ball b){
	return EPS < sqdist(b,start) - sqdist(a,start);
};
Пример #10
0
QList<Polygon> Polygon::convexPartition() const { // precondition: ccw; see mnbayazit.com/406/bayazit for details about how this works
    QList<Polygon> list;
    qreal d, dist1, dist2;
    QPointF ip, ip1, ip2; // intersection points
    int ind1, ind2;
    Polygon poly1, poly2;

    for(int i = 0; i < size(); ++i) {
        if(reflex(i)) {
            dist1 = dist2 = std::numeric_limits<qreal>::max();
            for(int j = 0; j < size(); ++j) {
                if(left(at(i - 1), at(i), at(j)) && rightOn(at(i - 1), at(i), at(j - 1))) { // if ray (i-1)->(i) intersects with edge (j, j-1)
                    QLineF(at(i - 1), at(i)).intersect(QLineF(at(j), at(j - 1)), &ip);
                    if(right(at(i + 1), at(i), ip)) { // intersection point isn't caused by backwards ray
                        d = sqdist(at(i), ip);
                        if(d < dist1) { // take the closest intersection so we know it isn't blocked by another edge
                            dist1 = d;
                            ind1 = j;
                            ip1 = ip;
                        }
                    }
                }
                if(left(at(i + 1), at(i), at(j + 1)) && rightOn(at(i + 1), at(i), at(j))) { // if ray (i+1)->(i) intersects with edge (j+1, j)
                    QLineF(at(i + 1), at(i)).intersect(QLineF(at(j), at(j + 1)), &ip);
                    if(left(at(i - 1), at(i), ip)) {
                        d = sqdist(at(i), ip);
                        if(d < dist2) {
                            dist2 = d;
                            ind2 = j;
                            ip2 = ip;
                        }
                    }
                }
            }
            if(ind1 == (ind2 + 1) % size()) { // no vertices in range
                QPointF sp((ip1 + ip2) / 2);
                poly1 = copy(i, ind2);
                poly1.append(sp);
                poly2 = copy(ind1, i);
                poly2.append(sp);
            } else {
                double highestScore = 0, bestIndex = ind1, score;
                while(ind2 < ind1) ind2 += size();
                for(int j = ind1; j <= ind2; ++j) {
                    if(canSee(i, j)) {
                        score = 1 / (sqdist(at(i), at(j)) + 1);
                        if(reflex(j)) {
                            if(rightOn(at(j - 1), at(j), at(i)) && leftOn(at(j + 1), at(j), at(i))) {
                                score += 3;
                            } else {
                                score += 2;
                            }
                        } else {
                            score += 1;
                        }
                        if(score > highestScore) {
                            bestIndex = j;
                            highestScore = score;
                        }
                    }
                }
                poly1 = copy(i, bestIndex);
                poly2 = copy(bestIndex, i);
            }
            list += poly1.convexPartition();
            list += poly2.convexPartition();
            return list;
        }
    }
    // polygon is already convex
    if(size() > b2_maxPolygonVertices) {
        poly1 = copy(0, size() / 2);
        poly2 = copy(size() / 2, 0);
        list += poly1.convexPartition();
        list += poly2.convexPartition();
    } else list.append(*this);
    return list;
}