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);
}
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;
}
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;
// }
}
}
}
}
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);
}
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) ;
}
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();
}
}
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;
}
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;
}
bool closerToStart(Ball a, Ball b){
return EPS < sqdist(b,start) - sqdist(a,start);
};
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;
}