Sphere Sphere::calculateBoundingSphere( const vec3 *points, size_t numPoints ) { if( ! numPoints ) return Sphere( vec3( 0 ), 0 ); // compute minimal and maximal bounds vec3 min(points[0]), max(points[0]); for( size_t i = 1; i < numPoints; ++i ) { if( points[i].x < min.x ) min.x = points[i].x; else if( points[i].x > max.x ) max.x = points[i].x; if( points[i].y < min.y ) min.y = points[i].y; else if( points[i].y > max.y ) max.y = points[i].y; if( points[i].z < min.z ) min.z = points[i].z; else if( points[i].z > max.z ) max.z = points[i].z; } // compute center and radius vec3 center = 0.5f * ( min + max ); float maxDistance = distance2( center, points[0] ); for( size_t i = 1; i < numPoints; ++i ) { float dist = distance2( center, points[i] ); if( dist > maxDistance ) maxDistance = dist; } return Sphere( center, math<float>::sqrt( maxDistance ) ); }
/** Recherche les 3 premiers points non aligné */ bool Convexe2D::initNonAlignee(const QVector<Vector2D>& points, int& i0, int& i1, int& i2) const { i0 = 0; i1 = 1; i2 = 2; Vector2D p0 = points[i0], p1 = points[i1]; while(i2 < points.size()) { Vector2D p2 = points[i2]; if(inHalfSpaceDroit(p0,p1,p2) != ALIGNEE) return true; else { //mettre p2 entre p0 et p1 if(distance2(p0, p1) < distance2(p0, p2)){ // p0 -- p1 -- p2 ou p1 -- p0 ---- p2 int swap = i2; i2 = i1; i1 = swap; p1 = points[i1]; p2 = points[i2]; // p0 -- p2 --- p1 ou p2 -- p0 ---- p1 } if(distance2(p0, p1) < distance2(p1, p2)) //p2 -- p0 ---- p1 { int swap = i0; i0 = i2; i2 = swap; p0 = points[i0]; //p0 -- p2 ---- p1 //p2 = points[i2]; } i2++; } } return false; //on ne peut pas construire un triangle avec les points, ils sont tous aligné }
void Quadtree::update(GLdouble x, GLdouble y, GLdouble z) { bool split = distance2(x, y, z) < (box[2] - box[0]) * (box[2] - box[0]) * 4.; if (split) { if (children[0] == NULL && level > 0) { divide(); } if (children[0] != NULL) { children[0]->update(x, y, z); children[1]->update(x, y, z); children[2]->update(x, y, z); children[3]->update(x, y, z); } } else if (children[0] != NULL) { delete children[0]; delete children[1]; delete children[2]; delete children[3]; children[0] = NULL; children[1] = NULL; children[2] = NULL; children[3] = NULL; } if (distance2(x, y, z) < distance * distance) { distance = sqrt(distance2(x, y, z)); } }
void Features::makeBasicFeature(int offset, const Node *first, const Node *last) { // distance addFeature(offset + 1 , 10 * distance(first, last)); // degree addFeature(offset + 2 , std::atan2(last->y - first->y, last->x - first->x)); // absolute position addFeature(offset + 3, 10 * (first->x - 0.5)); addFeature(offset + 4, 10 * (first->y - 0.5)); addFeature(offset + 5, 10 * (last->x - 0.5)); addFeature(offset + 6, 10 * (last->y - 0.5)); // absolute degree addFeature(offset + 7, std::atan2(first->y - 0.5, first->x - 0.5)); addFeature(offset + 8, std::atan2(last->y - 0.5, last->x - 0.5)); // absolute distance addFeature(offset + 9, 10 * distance2(first)); addFeature(offset + 10, 10 * distance2(last)); // diff addFeature(offset + 11, 5 * (last->x - first->x)); addFeature(offset + 12, 5 * (last->y - first->y)); }
real calc_geom(int isize, atom_id *index, rvec *x, rvec geom_center, rvec min, rvec max, gmx_bool bDiam) { real diam2, d; char *grpnames; int ii, i, j; clear_rvec(geom_center); diam2 = 0; if (isize == 0) { clear_rvec(min); clear_rvec(max); } else { if (index) ii = index[0]; else ii = 0; for (j = 0; j < DIM; j++) min[j] = max[j] = x[ii][j]; for (i = 0; i < isize; i++) { if (index) ii = index[i]; else ii = i; rvec_inc(geom_center, x[ii]); for (j = 0; j < DIM; j++) { if (x[ii][j] < min[j]) min[j] = x[ii][j]; if (x[ii][j] > max[j]) max[j] = x[ii][j]; } if (bDiam) { if (index) for (j = i + 1; j < isize; j++) { d = distance2(x[ii], x[index[j]]); diam2 = max(d,diam2); } else for (j = i + 1; j < isize; j++) { d = distance2(x[i], x[j]); diam2 = max(d,diam2); } } } svmul(1.0 / isize, geom_center, geom_center); } return sqrt(diam2); }
void callback(JointsSet& collection, const brics_actuator::JointPositionsConstPtr& msg) {//Des nouvelles coordonnées ont été publiées Joints coord_recues = { msg->positions[0].value, msg->positions[1].value, msg->positions[2].value, msg->positions[3].value }; auto min_iter = std::min_element(collection.begin(), collection.end(), boost::bind(distance2,std::ref(coord_recues),_1)); // Si Ok on l'enregistre if(distance2(coord_recues,*min_iter) > MIN_DIST_THRESHOLD*MIN_DIST_THRESHOLD) { std::ofstream fichier; fichier.exceptions(std::ios::failbit | std::ios::badbit); try { fichier.open(adresseTexte.c_str(), std::ios_base::app); fichier << coord_recues << std::endl; fichier.close(); collection.push_back(coord_recues); ROS_INFO_STREAM("Coordonnee enregistree"); std::cout << coord_recues<<std::endl; } catch (const std::exception& e) { ROS_INFO_STREAM("ERREUR Ouverture Fichier"); } } }
int mergecb(float *B,float **cb,unsigned char *P,int npt,int N,float thresh,int dim) { int i,j,newN,*count,l,ei,debug=0; float **dist; if (N==1) return N; count=(int *)malloc(sizeof(int)*N); for (l=0;l<npt;l++) count[P[l]]++; dist=(float **)fmatrix(N,N); ei = N-1; for (i=0;i<ei;i++) { for (j=i+1;j<N;j++) { dist[i][j] = distance2(cb[i],cb[j],dim); dist[j][i] = dist[i][j]; } } if (debug) printf("thresh %f ",thresh); newN=mergecb1(dist,B,cb,N,thresh,dim,count); free_fmatrix(dist,N); free(count); return newN; }
// implementation of the function to be minimized double operator() (const double * par) { assert(fGraph != 0); double * x = fGraph->GetX(); double * y = fGraph->GetY(); double * z = fGraph->GetZ(); int npoints = fGraph->GetN(); double sum = 0; for (int i = 0; i < npoints; ++i) { double d = distance2(x[i],y[i],z[i],par); sum += d; #ifdef DEBUG if (first) std::cout << "point " << i << "\t" << x[i] << "\t" << y[i] << "\t" << z[i] << "\t" << std::sqrt(d) << std::endl; #endif } // if (first) // std::cout << "Total distance square w/ initial params: " << sum << std::endl; // first = false; return sum; }
multimap<int,double> Mesh<Ttype, Tpos>::getNearMeshMap(Tpos x, Tpos y, Tpos z, Tpos rmax, Tpos rmin) { std::multimap<int, double> nbr; // the return list nbr.clear(); // clear list int ix,iy,iz,ii,p; Tpos dst2; ix = int((x - xmin)/dx); // calculate index of point iy = int((y - ymin)/dy); iz = int((z - zmin)/dz); int srx = int(rmax/dx)+1; // calculate radius in terms of mesh indicies int sry = int(rmax/dy)+1; int srz = int(rmax/dz)+1; std::vector<int> close = closemeshes(ix,iy,iz,srx,sry,srz); for (std::vector<int>::iterator ii=close.begin(); ii!=close.end(); ii++) { if ( (p=head[*ii])>=0 ) { do { dst2 = distance2((*dat)[p]->getX(), (*dat)[p]->getY(), (*dat)[p]->getZ(), x, y, z); if (dst2<rmax*rmax && dst2>=rmin*rmin) { nbr.insert(std::pair<double,int>(p,dst2)); } } while( (p=next[p])>=0 ); } } return(nbr); }
list<Tpos> Mesh<Ttype, Tpos>::getDistSqList(Tpos x, Tpos y, Tpos z, Tpos rmax, Tpos rmin) { // Returns a list of distances^2 for objects near pos. std::list<Tpos> nbr; nbr.clear(); int ix,iy,iz,ii,p; Tpos dr,dst2; ix = int((x - xmin)/dx); // calculate index of point iy = int((y - ymin)/dy); iz = int((z - zmin)/dz); int srx = int(rmax/dx)+1; // calculate radius in terms of mesh indicies int sry = int(rmax/dy)+1; int srz = int(rmax/dz)+1; std::vector<int> close = closemeshes(ix,iy,iz,srx,sry,srz); for (std::vector<int>::iterator ii=close.begin(); ii!=close.end(); ii++) { if ( (p=head[*ii])>=0 ) { do { dst2=0; dst2 = distance2((*dat)[p]->getX(), (*dat)[p]->getY(), (*dat)[p]->getZ(), x, y, z); if (dst2<rmax*rmax && dst2>rmin*rmin) { nbr.push_back(dst2); } } while( (p=next[p])>=0 ); } } return(nbr); }
void solve(){ memset(matches,0,sizeof(matches)); // printf("%d\n",sizeof(matches)); memset(links,0,sizeof(links)); memset(ans,0,sizeof(ans)); for(int i=1;i<n;i++){ double distOfI = 2*distance1(i); for(int j = 1;j<=m;j++){ if(distance2(i,j)<=distOfI){ matches[i][j] = 1; } } } int s = n; for(int i=1;i<=n;i++){ memset(visited,0,sizeof(visited)); s+=dfs(i); } printf("%d\n",s); for(int i=1;i<n;i++){ printf("%d %d ",routes[i][0],routes[i][1]); if(ans[i]){ printf("%d %d ",places[ans[i]][0],places[ans[i]][1]); } } printf("%d %d",routes[n][0],routes[n][1]); printf("\n"); }
PyObject *wrap_distance2(PyObject *self,PyObject *args) { PyObject *cs1, *cs2; PyObject *fast1, *fast2; if(!PyArg_ParseTuple(args,"OO",&cs1,&cs2)) return NULL; if(!(fast1 = PySequence_Fast(cs1, "could not get fast sequence"))) return NULL; if(!(fast2 = PySequence_Fast(cs2, "could not get fast sequence"))) { return NULL; } rvec r1,r2; int i; PyObject *d1,*d2; for(i=0;i<DIM;i++){ d1=PySequence_Fast_GET_ITEM(fast1,i); d2=PySequence_Fast_GET_ITEM(fast2,i); r1[i] = PyFloat_AsDouble(d1); r2[i] = PyFloat_AsDouble(d2); } real d = distance2(r1,r2); PyObject *ret; ret=Py_BuildValue("d",d); return ret; }
/* Compute the exact integration of the mipmap \a M within the ball of radius r and center (x0,y0,z0). The method is a simple scanning at the finest scale. */ Value computeExact( MipMap* M, int x0, int y0, int z0, Value r ) { Image* img = MipMap_get_image( M, M->max_lvl ); int minx = x0 - (int) ceil( r ); int miny = y0 - (int) ceil( r ); int minz = z0 - (int) ceil( r ); int maxx = x0 + (int) ceil( r ); int maxy = y0 + (int) ceil( r ); int maxz = z0 + (int) ceil( r ); minx = minx <= 0 ? 0 : minx; miny = miny <= 0 ? 0 : miny; minz = minz <= 0 ? 0 : minz; maxx = maxx >= img->size ? img->size-1 : maxx; maxy = maxy >= img->size ? img->size-1 : maxy; maxz = maxz >= img->size ? img->size-1 : maxz; Value r2 = r*r; Value acc = (Value) 0; for ( int z = minz; z <= maxz; ++z ) for ( int y = miny; y <= maxy; ++y ) for ( int x = minx; x <= maxx; ++x ) { if ( distance2( x0, y0, z0, x, y, z ) <= r2 ) { acc += Image_get( img, x, y, z ); nb_access_exact += 1; } nb_iteration_exact += 1; } return acc; }
double Atomtype::CalcPE(int frame_i, const Trajectory &trj, const coordinates &rand_xyz, const cubicbox_m256 &box, double vol) const { float pe = 0.0; int atom_i = 0; /* BEGIN SIMD SECTION */ // This performs the exact same calculation after the SIMD section // but doing it on 8 atoms at a time using SIMD instructions. coordinates8 rand_xyz8(rand_xyz), atom_xyz; __m256 r2_8, mask, r6, ri6, pe_tmp; __m256 pe_sum = _mm256_setzero_ps(); float result[n] __attribute__((aligned (16))); for (; atom_i < this->n-8; atom_i+=8) { atom_xyz = trj.GetXYZ8(frame_i, this->name, atom_i); r2_8 = distance2(atom_xyz, rand_xyz8, box); mask = _mm256_cmp_ps(r2_8, rcut2_8, _CMP_LT_OS); r6 = _mm256_and_ps(mask, _mm256_mul_ps(_mm256_mul_ps(r2_8, r2_8), r2_8)); ri6 = _mm256_and_ps(mask, _mm256_rcp_ps(r6)); pe_tmp = _mm256_and_ps(mask, _mm256_mul_ps(ri6, _mm256_sub_ps(_mm256_mul_ps(c12_8, ri6), c6_8))); pe_sum = _mm256_add_ps(pe_tmp, pe_sum); } _mm256_store_ps(result, pe_sum); for (int i = 0; i < 8; i++) { pe += result[i]; } /* END SIMD SECTION */ for (; atom_i < this->n; atom_i++) { coordinates atom_xyz = trj.GetXYZ(frame_i, this->name, atom_i); float r2 = distance2(atom_xyz, rand_xyz, cubicbox(box)); if (r2 < this->rcut2) { float ri6 = 1.0/(pow(r2,3)); pe += ri6*(this->c12*ri6 - this->c6); } } pe += this->n/vol * this->tail_factor;; return pe; }
void getcmap(float *B,unsigned char *cmap,float **cb,int npt,int dim,int N) { int i,j; float mindif,dif; for (i=0;i<npt;i++) { mindif = distance2(B,cb[0],dim); cmap[i]=0; for (j=1;j<N;j++) { dif = distance2(B,cb[j],dim); if (dif<mindif) { cmap[i]=j; mindif=dif; } } B += dim; } }
//H-Client // TODO: should be more general int CGameClient::IntersectCharacter(vec2 Pos0, vec2 Pos1, vec2& NewPos, float Speed/*hook speed*/, int ownID) { // Find other players const float ProximityRadius2 = (28.f + 2.f) * (28.f + 2.f); float ClosestLen2 = distance2(Pos0, Pos1) * 10000.0f; int ClosestID = -1; if (!m_Tuning.m_PlayerHooking) return ClosestID; for (int i=0; i<MAX_CLIENTS; i++) { const CClientData& cData = m_aClients[i]; if (!cData.m_Active || cData.m_Team == TEAM_SPECTATORS || i == ownID || !m_Teams.SameTeam(i, ownID)) continue; vec2 Position = cData.m_Predicted.m_Pos; vec2 Velocity = cData.m_Predicted.m_Vel; vec2 IntersectPos = closest_point_on_line(Pos0, Pos1, Position); if(Speed != 0.f) { const float DistFromTarget = distance(Pos0, IntersectPos); // predict Position += Velocity * (DistFromTarget / Speed); // can be deleted ? IntersectPos = closest_point_on_line(Pos0, Pos1, Position); } float Len2 = distance2(Position, IntersectPos); if(Len2 < ProximityRadius2) { Len2 = distance2(Pos0, IntersectPos); if(Len2 < ClosestLen2) { NewPos = IntersectPos; ClosestLen2 = Len2; ClosestID = i; } } } return ClosestID; }
bool steepest_descent_step(void) { Cell *cell; Particle *p; int c, i, j, np; double f_max = -std::numeric_limits<double>::max(); double f; /* Verlet list criterion */ const double skin2 = SQR(0.5*skin); double f_max_global; double dx[3], dx2; const double max_dx2 = SQR(params->max_displacement); for (c = 0; c < local_cells.n; c++) { cell = local_cells.cell[c]; p = cell->part; np = cell->n; for(i = 0; i < np; i++) { f = 0.0; dx2 = 0.0; #ifdef VIRTUAL_SITES if (ifParticleIsVirtual(&p[i])) continue; #endif for(j=0; j < 3; j++){ #ifdef EXTERNAL_FORCES if (!(p[i].p.ext_flag & COORD_FIXED(j))) #endif { f += SQR(p[i].f.f[j]); dx[j] = params->gamma * p[i].f.f[j]; dx2 += SQR(dx[j]); MINIMIZE_ENERGY_TRACE(printf("part %d dim %d dx %e gamma*f %e\n", i, j, dx[j], params->gamma * p[i].f.f[j])); } #ifdef EXTERNAL_FORCES else { dx[j] = 0.0; } #endif } if(dx2 <= max_dx2) { p[i].r.p[0] += dx[0]; p[i].r.p[1] += dx[1]; p[i].r.p[2] += dx[2]; } else { const double c = params->max_displacement/std::sqrt(dx2); p[i].r.p[0] += c*dx[0]; p[i].r.p[1] += c*dx[1]; p[i].r.p[2] += c*dx[2]; } if(distance2(p[i].r.p,p[i].l.p_old) > skin2 ) resort_particles = 1; f_max = std::max(f_max, f); } } MINIMIZE_ENERGY_TRACE(printf("f_max %e resort_particles %d\n", f_max, resort_particles)); announce_resort_particles(); MPI_Allreduce(&f_max, &f_max_global, 1, MPI_DOUBLE, MPI_MAX, comm_cart); return (sqrt(f_max_global) < params->f_max); }
static void cellFunc(const R3 &p, float &n0, float &f0, float &f1) { R3 q = p; int i = (int)floorf(q.x); int j = (int)floorf(q.y); int k = (int)floorf(q.z); q.x -= i; q.y -= j; q.z -= k; int index = fold(i,j,k); float minDist = distance2(CellSampleTable[index], q); float minDist2 = 2.0; int k0; k0 = index; // easy but slow way, check distance to point in each adjacent // cell to find closest and second closest. R3 q1; for (int ii = -1; ii <= 1; ++ii) { q1.x = q.x - ii; int i1 = i + ii; for (int jj = -1; jj <= 1; ++jj) { q1.y = q.y - jj; int j1 = j + jj; for (int kk = -1; kk <= 1; ++kk) { if (!ii && !jj && !kk) continue; q1.z = q.z - kk; int k1 = k + kk; index = fold(i1, j1, k1); float t = distance2(CellSampleTable[index], q1); if (minDist > t) { minDist2 = minDist; minDist = t; k0 = index; } else if (minDist2 > t) { minDist2 = t; } } } } f0 = sqrtf(minDist); f1 = sqrtf(minDist2); n0 = k0/(float)(N_CELLS); }
float Sphere::distance(const Vector3D& p) const { float dst2 = distance2(centre, p); if(dst2 > rayon*rayon) return sqrt(dst2)-rayon; else return 0; }
real dist2_from_atoms( PyObject *atom1, PyObject *atom2) { PyObject *Ox1 = PyObject_GetAttrString(atom1, "x"); PyObject *Ox2 = PyObject_GetAttrString(atom2, "x"); rvec x1, x2; Pyvec2rvec( Ox1, x1); Pyvec2rvec( Ox2, x2); return distance2(x1, x2 ); }
static double helixDistance2(HelixInfo bot, HelixInfo top, double delta_z, double delta_phi) { double dz = top.z - bot.z + delta_z; double dphi = top.phi - bot.phi + delta_phi; Vec3 low = (Vec3) {bot.r, 0, 0}; Vec3 high = (Vec3) {top.r * cos(dphi), top.r * sin(dphi), dz}; return distance2(low, high); }
int pCompare( const void *a, const void *b ) { int wise; unsigned d, d2; wise = cross( min, *( point* )a, *( point* )b ); if ( wise > 0 ) { return -1; } if ( wise < 0 ) { return 1; } d = distance2( min, *( point* )a ); d2 = distance2( min, *( point* )b ); if ( d < d2 ) { return -1; } return 1; }
/* find the closest local minimizer (lm) to p with a smaller function value; this function is called when p is first added to our tree */ static void find_closest_lm(int n, rb_tree *lms, pt *p) { rb_node *node = rb_tree_find_lt(lms, &p->f); double closest_d = HUGE_VAL; while (node) { double d = distance2(n, p->x, node->k+1); if (d < closest_d) closest_d = d; node = rb_tree_pred(node); } p->closest_lm_d = closest_d; }
/* find the closest pt to p with a smaller function value; this function is called when p is first added to our tree */ static void find_closest_pt(int n, rb_tree *pts, pt *p) { rb_node *node = rb_tree_find_lt(pts, (rb_key) p); double closest_d = HUGE_VAL; while (node) { double d = distance2(n, p->x, ((pt *) node->k)->x); if (d < closest_d) closest_d = d; node = rb_tree_pred(node); } p->closest_pt_d = closest_d; }
/* given newpt, which presumably has just been added to our tree, update all pts with a greater function value in case newpt is closer to them than their previous closest_pt ... we can ignore already-minimized points since we never do local minimization from the same point twice */ static void pts_update_newpt(int n, rb_tree *pts, pt *newpt) { rb_node *node = rb_tree_find_gt(pts, (rb_key) newpt); while (node) { pt *p = (pt *) node->k; if (!p->minimized) { double d = distance2(n, newpt->x, p->x); if (d < p->closest_pt_d) p->closest_pt_d = d; } node = rb_tree_succ(node); } }
static real max_dist(rvec *x, real *r, int start, int end) { real maxd; int i,j; maxd=0; for(i=start; i<end; i++) for(j=i+1; j<end; j++) maxd=max(maxd,sqrt(distance2(x[i],x[j]))+0.5*(r[i]+r[j])); return 0.5*maxd; }
void SumDistance2(int &, double *, double & sum, double * par, int ) { TGraph2D * m_gr = dynamic_cast<TGraph2D*>( (TVirtualFitter::GetFitter())->GetObjectFit() ); assert (m_gr != 0); double * px = m_gr->GetX(); double * py = m_gr->GetY(); double * pz = m_gr->GetZ(); int np = m_gr->GetN(); sum = 0; for (int i = 0; i < np; ++i) { double d = distance2(px[i],py[i],pz[i],par); sum += d; } }
void Image_ball( Image* img, int x0, int y0, int z0, float r ) { Value r2 = r*r; Value* data = img->data; for ( int z = 0; z < img->size; ++z ) for ( int y = 0; y < img->size; ++y ) for ( int x = 0; x < img->size; ++x ) { if ( distance2( x0, y0, z0, x, y, z ) <= r2 ) *data++ = (Value) 1; else *data++ = (Value) 0; } }
void GrabCut::computeBeta() { Real result = 0; int edges = 0; for (unsigned int y = 0; y < m_h; ++y) { for (unsigned int x = 0; x < m_w; ++x) { if (x > 0 && y < m_h-1) // upleft { result += distance2( (*m_image)(x,y), (*m_image)(x-1,y+1) ); edges++; } if (y < m_h-1) // up { result += distance2( (*m_image)(x,y), (*m_image)(x,y+1) ); edges++; } if (x < m_w-1 && y < m_h-1) // upright { result += distance2( (*m_image)(x,y), (*m_image)(x+1,y+1) ); edges++; } if (x < m_w-1) // right { result += distance2( (*m_image)(x,y), (*m_image)(x+1,y) ); edges++; } } } m_beta = (Real)(1.0/(2*result/edges)); }
/* given newlm, which presumably has just been added to our tree, update all pts with a greater function value in case newlm is closer to them than their previous closest_lm ... we can ignore already-minimized points since we never do local minimization from the same point twice */ static void pts_update_newlm(int n, rb_tree *pts, double *newlm) { pt tmp_pt; rb_node *node; tmp_pt.f = newlm[0]; node = rb_tree_find_gt(pts, (rb_key) &tmp_pt); while (node) { pt *p = (pt *) node->k; if (!p->minimized) { double d = distance2(n, newlm+1, p->x); if (d < p->closest_lm_d) p->closest_lm_d = d; } node = rb_tree_succ(node); } }