void ofxPolygonMask::mousePressed(ofMouseEventArgs &m) { if(doPoint) { focusedPoint = insertPoint(ofVec3f(m.x, m.y, 20)); selectedPoint = focusedPoint; return; } else if(doDelete) { for(int i = 0; i < points.size(); i++) { if(sqrDist(m.x, m.y, points[i].x, points[i].y)<SQR_DIST_POINT_GRAB) { // 5 pixels distance points.erase(points.begin()+i); return; } } } for(int i = 0; i < points.size(); i++) { if(sqrDist(m.x, m.y, points[i].x, points[i].y)<SQR_DIST_POINT_GRAB) { // 5 pixels distance selectedPoint = i; focusedPoint = i; //points[selectedPoint].x = m.x; //points[selectedPoint].y = m.y; } } }
size_t PointVec::uniqueInsert(Point* pnt) { size_t n(_data_vec->size()), k; const double eps(std::numeric_limits<double>::epsilon()); for (k = 0; k < n; k++) if (fabs((*((*_data_vec)[k]))[0] - (*pnt)[0]) < eps && fabs((*((*_data_vec)[k]))[1] - (*pnt)[1]) < eps && fabs((*((*_data_vec)[k]))[2] - (*pnt)[2]) < eps) break; if (k == n) { _data_vec->push_back(pnt); // update bounding box _aabb.update(*((*_data_vec)[n])); // update shortest distance for (size_t i(0); i < n; i++) { double sqr_dist(sqrDist((*_data_vec)[i], (*_data_vec)[n])); if (sqr_dist < _sqr_shortest_dist) _sqr_shortest_dist = sqr_dist; } return n; } delete pnt; pnt = NULL; return k; }
double calcProjPntToLineAndDists(const double p[3], const double a[3], const double b[3], double &lambda, double &d0) { // g (lambda) = a + lambda v, v = b-a double v[3] = {b[0] - a[0], b[1] - a[1], b[2] - a[2]}; // orthogonal projection: (g(lambda)-p) * v = 0 => in order to compute lambda we define a help vector u double u[3] = {p[0] - a[0], p[1] - a[1], p[2] - a[2]}; lambda = scpr<double,3> (u, v) / scpr<double,3> (v, v); // compute projected point double proj_pnt[3]; for (size_t k(0); k<3; k++) proj_pnt[k] = a[k] + lambda * v[k]; d0 = sqrt (sqrDist (proj_pnt, a)); return sqrt (sqrDist (p, proj_pnt)); }
void AutoTriangleMesh<PointType>::limitEdgeLength(const typename AutoTriangleMesh<PointType>::BasePoint& center,double radius,double maxEdgeLength) { /* Iterate through all triangles: */ FaceIterator faceIt=BaseMesh::beginFaces(); while(faceIt!=BaseMesh::endFaces()) { /* Check whether face overlaps area of influence and calculate face's maximum edge length: */ bool overlaps=false; Edge* longestEdge=0; double longestEdgeLength2=maxEdgeLength*maxEdgeLength; Edge* e=faceIt->getEdge(); for(int i=0;i<3;++i) { overlaps=overlaps||sqrDist(*e->getStart(),center)<=radius*radius; /* Calculate edge's squared length: */ double edgeLength2=sqrDist(*e->getStart(),*e->getEnd()); if(longestEdgeLength2<edgeLength2) { longestEdge=e; longestEdgeLength2=edgeLength2; } /* Go to next edge: */ e=e->getFaceSucc(); } /* Check whether the longest triangle edge is too long: */ if(overlaps&&longestEdge!=0) { /* Split longest edge: */ splitEdge(longestEdge); } else { /* Go to next triangle: */ ++faceIt; } } }
PointVec::PointVec(const std::string& name, std::vector<Point*>* points, std::map<std::string, size_t>* name_id_map, PointType type, double rel_eps) : TemplateVec<Point>(name, points, name_id_map), _type(type), _sqr_shortest_dist(std::numeric_limits<double>::max()) { assert(_data_vec); size_t number_of_all_input_pnts(_data_vec->size()); calculateAxisAlignedBoundingBox(); rel_eps *= sqrt(sqrDist(&(_aabb.getMinPoint()), &(_aabb.getMaxPoint()))); makePntsUnique(_data_vec, _pnt_id_map, rel_eps); if (number_of_all_input_pnts - _data_vec->size() > 0) std::cerr << "WARNING: there are " << number_of_all_input_pnts - _data_vec->size() << " double points" << "\n"; }
double QgsPoint::sqrDistToSegment( double x1, double y1, double x2, double y2, QgsPoint& minDistPoint, double epsilon ) const { double nx, ny; //normal vector nx = y2 - y1; ny = -( x2 - x1 ); double t; t = ( m_x * ny - m_y * nx - x1 * ny + y1 * nx ) / (( x2 - x1 ) * ny - ( y2 - y1 ) * nx ); if ( t < 0.0 ) { minDistPoint.setX( x1 ); minDistPoint.setY( y1 ); } else if ( t > 1.0 ) { minDistPoint.setX( x2 ); minDistPoint.setY( y2 ); } else { minDistPoint.setX( x1 + t *( x2 - x1 ) ); minDistPoint.setY( y1 + t *( y2 - y1 ) ); } double dist = sqrDist( minDistPoint ); //prevent rounding errors if the point is directly on the segment if ( qgsDoubleNear( dist, 0.0, epsilon ) ) { minDistPoint.setX( m_x ); minDistPoint.setY( m_y ); return 0.0; } return dist; }
double QgsPoint::distance( double x, double y ) const { return sqrt( sqrDist( x, y ) ); }
double QgsPoint::sqrDist( const QgsPoint& other ) const { return sqrDist( other.x(), other.y() ); }
std::vector<poseT> RefinePoses(const pcl::PointCloud<myPointXYZ>::Ptr scene, const std::vector<ModelT> &mesh_set, const std::vector<poseT> &all_poses) { int pose_num = all_poses.size(); std::vector<ModelT> est_models(pose_num); pcl::PointCloud<myPointXYZ>::Ptr down_scene(new pcl::PointCloud<myPointXYZ>()); pcl::VoxelGrid<myPointXYZ> sor; sor.setInputCloud(scene); sor.setLeafSize(0.005, 0.005, 0.005); sor.filter(*down_scene); #pragma omp parallel for schedule(dynamic, 1) for(int i = 0 ; i < pose_num ; i++ ){ for( int j = 0 ; j < mesh_set.size() ; j++ ){ if( mesh_set[j].model_label == all_poses[i].model_name ) { est_models[i].model_label = all_poses[i].model_name; est_models[i].model_cloud = pcl::PointCloud<myPointXYZ>::Ptr (new pcl::PointCloud<myPointXYZ>()); pcl::transformPointCloud(*mesh_set[j].model_cloud, *est_models[i].model_cloud, all_poses[i].shift, all_poses[i].rotation); break; } } } std::vector< pcl::search::KdTree<myPointXYZ>::Ptr > tree_set(est_models.size()); #pragma omp parallel for schedule(dynamic, 1) for( int i = 0 ; i < pose_num ; i++ ) { tree_set[i] = pcl::search::KdTree<myPointXYZ>::Ptr (new pcl::search::KdTree<myPointXYZ>()); tree_set[i]->setInputCloud(est_models[i].model_cloud); } std::vector<int> votes(pose_num, 0); std::vector< std::vector<int> > adj_graph(pose_num); for( int i = 0 ; i < pose_num ; i++ ) adj_graph[i].resize(pose_num, 0); float sqrT = 0.01*0.01; int down_num = down_scene->size(); std::vector< std::vector<int> > bin_vec(down_num); #pragma omp parallel for for(int i = 0 ; i < pose_num ; i++ ) { int count = 0; for( pcl::PointCloud<myPointXYZ>::const_iterator it = down_scene->begin() ; it < down_scene->end() ; it++, count++ ) { std::vector<int> idx (1); std::vector<float> sqrDist (1); int nres = tree_set[i]->nearestKSearch(*it, 1, idx, sqrDist); if ( nres >= 1 && sqrDist[0] <= sqrT ) { #pragma omp critical { bin_vec[count].push_back(i); } votes[i]++; } } } for( int it = 0 ; it < down_num ; it++ ) for( std::vector<int>::iterator ii = bin_vec[it].begin() ; ii < bin_vec[it].end() ; ii++ ) for( std::vector<int>::iterator jj = ii+1 ; jj < bin_vec[it].end() ; jj++ ) { adj_graph[*ii][*jj]++; adj_graph[*jj][*ii]++; } std::vector<bool> dead_flag(pose_num, 0); for( int i = 0 ; i < pose_num ; i++ ){ if( dead_flag[i] == true ) continue; for( int j = i+1 ; j < pose_num ; j++ ) { if( dead_flag[j] == true ) continue; int min_tmp = std::min(votes[i], votes[j]); if( (adj_graph[i][j]+0.0) / min_tmp >= 0.75 ) { if( votes[i] > votes[j] ) dead_flag[j] = true; else { dead_flag[i] = true; break; } } } } std::vector<poseT> refined_poses; for( int i = 0 ; i < pose_num ; i++ ) if( dead_flag[i] == false ) refined_poses.push_back(all_poses[i]); return refined_poses; }
bool checkDistance(GeoLib::Point const &p0, GeoLib::Point const &p1, double squaredDistance) { return (sqrDist(&p0, &p1) < squaredDistance); }
int searchKNNVPNode(VPNode *vpNode, double *queryPt, double *kMinDist, double *resultPt, int *resultID, int numBranch, int dimension, int k) { double dist, *ptrPt; int i, x, y; int index, nodeCount; // visited me nodeCount = 1; // what kind of node? if (vpNode->isLeaf) { ptrPt = vpNode->points; for (i=0; i<vpNode->nPoints; i++) { // compute distance dist = sqrt( sqrDist(queryPt,ptrPt,dimension) ); // shorter? if (kMinDist[k-1] > dist) { for (x=0; x<k-1; x++) if (kMinDist[x] > dist) break; index = x; for (x=k-1; x>index; x--) { kMinDist[x] = kMinDist[x-1]; resultID[x] = resultID[x-1]; for (y=0; y<dimension; y++) resultPt[x*dimension+y] = resultPt[(x-1)*dimension+y]; } kMinDist[index] = dist; memcpy(&(resultPt[index*dimension]),ptrPt,sizeof(double)*dimension); resultID[index] = vpNode->dataID[i]; } // next ptrPt += dimension; } } else { // compute distance dist = sqrt( sqrDist(queryPt,vpNode->points,dimension) ); // shorter? if (kMinDist[k-1] > dist) { for (i=0; i<k-1; i++) if (kMinDist[i] > dist) break; index = i; for (i=k-1; i>index; i--) { kMinDist[i] = kMinDist[i-1]; resultID[i] = resultID[i-1]; for (x=0; x<dimension; x++) resultPt[i*dimension+x] = resultPt[(i-1)*dimension+x]; } kMinDist[index] = dist; memcpy(&(resultPt[index*dimension]),vpNode->points,sizeof(double)*dimension); resultID[index] = vpNode->dataID[0]; } // Prune the near part for (i=0; i<numBranch-1; i++) if (kMinDist[k-1] + vpNode->medians[i] >= dist) break; // Prune the far part for (; i<numBranch; i++) { nodeCount += searchKNNVPNode( vpNode->child[i], queryPt, kMinDist, resultPt, resultID, numBranch, dimension, k ); if ( i != numBranch-1 && kMinDist[k-1] + dist < vpNode->medians[i] ) break; } } return nodeCount; }
VPNode * buildVPNode(double *points, int *dataID, int nPoints, int numBranch, int dimension) { VPNode *vpNode; int i,j; double *distances; int nSubset; int *subset; double stddev,bestStddev,dist; int vp,bestVP; double *ptrVP, *ptrPt, *ptrDist; int *sortList; double *tmpPt; int *tmpID, *ptrID; int lastIndex,childIndex; #ifdef _DEBUG printf("\nEnter buildVPNode (nPoints : %d) and (address %d)\n", nPoints,points); printf("dataID : %d\n",dataID[0]); #endif ///////////////////////////////////////////// // (1) Who am I? vpNode = (VPNode *) malloc(sizeof(VPNode)); if (vpNode == NULL) errexit("Err : Not enough memory for allocation!\n\n"); ///////////////////////////////////////////// // (2) Fill in information // avoid leakage when free() vpNode->child = NULL; vpNode->medians = NULL; vpNode->points = NULL; vpNode->dataID = NULL; // should we stop? if (nPoints <= _MAX_POINTS_NODE) { vpNode->isLeaf = _TRUE; // alloc. data points in leaf node vpNode->nPoints = nPoints; vpNode->points = (double *) malloc(sizeof(double)*dimension*nPoints); vpNode->dataID = (int *) malloc(sizeof(int)*nPoints); if (!vpNode->points || !vpNode->dataID) errexit("Err : Not enough memory for allocation!\n\n"); memcpy(vpNode->points, points, sizeof(double)*dimension*nPoints); memcpy(vpNode->dataID, dataID, sizeof(int)*nPoints); return vpNode; } else { vpNode->isLeaf = _FALSE; // alloc. vp vpNode->nPoints = 1; vpNode->points = (double *) malloc(sizeof(double)*dimension); vpNode->dataID = (int *) malloc(sizeof(int)); if (!vpNode->points || !vpNode->dataID) errexit("Err : Not enough memory for allocation!\n\n"); // * vp to be found later } // Initialize distances distances = (double *) malloc(sizeof(double)*(nPoints-1)); if (!distances) errexit("Err : Not enough memory for allocation!\n\n"); ///////////////////////////////////////////// // (3) Find the vantage point // first find a random "subset" in the dataset if (nPoints <= _NUM_PT_RANDOM_SUBSET) { // no need to find a random subset nSubset = nPoints; subset = (int *) malloc(sizeof(int)*nSubset); if (subset == NULL) errexit("Err : Not enough memory for allocation!\n\n"); for (i=0; i < nSubset; i++) subset[i] = i; } else { // find a random subset nSubset = _NUM_PT_RANDOM_SUBSET; subset = (int *) malloc(sizeof(int)*nSubset); if (subset == NULL) errexit("Err : Not enough memory for allocation!\n\n"); srand(time(NULL)); for (i=0; i < nSubset; i++) subset[i] = rand() % nPoints; } // Randomize some candidate vps and the one // with the largest standard dev. will be the VP srand(time(NULL)); // initialize bestStddev = 0.0; bestVP = -1; for (j=0; j < _NUM_CAND_VP; j++) { // randomize a candidate vp vp = rand() % nPoints; ptrVP = points + vp*dimension; // measure the standard dev. against this vp dist = 0.0; for (i=0; i < nSubset; i++) dist += sqrDist( ptrVP, points + subset[i]*dimension, dimension ); stddev = sqrt(dist/nSubset); // largest? if (bestStddev < stddev) { bestStddev = stddev; bestVP = vp; } } free(subset); #ifdef _DEBUG printf("bestVP : %d at (%f %f %f) with stdev %f\n", bestVP,ptrVP[0],ptrVP[1],ptrVP[2],bestStddev); #endif ///////////////////////////////////////////// // (4) Make the VP at the 1st // position by swapping // copy from (bestVP)th to vpNode memcpy(vpNode->points, points + bestVP*dimension, sizeof(double)*dimension); vpNode->dataID[0] = dataID[bestVP]; // copy from 0th element to (bestVP)th memcpy(points + bestVP*dimension, points, sizeof(double)*dimension); dataID[bestVP] = dataID[0]; // copy from vpNode to 0th element memcpy(points, vpNode->points, sizeof(double)*dimension); dataID[0] = vpNode->dataID[0]; ///////////////////////////////////////////// // (5) Find the distance of points to the vp ptrPt = points + dimension; // skip the 1st vp ptrDist = distances; for (i=0; i< nPoints-1; i++) { *ptrDist++ = sqrt( sqrDist(points, ptrPt, dimension) ); ptrPt += dimension; #ifdef _DEBUG printf("cal dist. %f (%f %f %f)\n",*(ptrDist-1), *(ptrPt-3),*(ptrPt-2),*(ptrPt-1)); #endif } ///////////////////////////////////////////// // (6) Sort the points in the array in the // ascending order of their corr. distances // initialize sortList = (int *) malloc(sizeof(int)*(nPoints-1)); if (sortList == NULL) errexit("Err : Not enough memory for allocation!\n\n"); for (i=0; i<nPoints-1; i++) sortList[i] = i; // sorting (find the sort index) //bubbleSortByIndex(&sortList, distances, nPoints-1); q_sort(sortList,distances,0,nPoints-2,nPoints-1); // rearrange the points -> tmpPt and dataID -> tmpID tmpPt = (double *) malloc(sizeof(double)*(nPoints-1)*dimension); tmpID = (int *) malloc(sizeof(int)*(nPoints-1)); if (!tmpPt || !tmpID) errexit("Err : Not enough memory for allocation!\n\n"); ptrPt = tmpPt; ptrID = tmpID; for (i=0; i<nPoints-1; i++) { memcpy( ptrPt, points + (sortList[i]+1)*dimension, // +1 to skip 1st vp sizeof(double)*dimension ); *ptrID++ = dataID[sortList[i]+1]; ptrPt += dimension; } // update points and dataID memcpy(points+dimension, tmpPt, sizeof(double)*(nPoints-1)*dimension); memcpy(dataID+1, tmpID, sizeof(int)*(nPoints-1)); free(tmpPt); free(tmpID); #ifdef _DEBUG for (i=0; i<nPoints-1; i++) printf("dist[%02d] = %f\n",i,distances[i]); for (i=0; i<nPoints-1; i++) printf("sort[%02d] = %d\n",i,sortList[i]); #endif ///////////////////////////////////////////// // (7) Fill in medians // initialize medians vpNode->medians = (double *) malloc(sizeof(double)*(numBranch-1)); if (!vpNode->medians) errexit("Err : Not enough memory for allocation!\n\n"); childIndex = 1; for (i=0; i<numBranch-1; i++) { // last point in this branch lastIndex = 1 + (nPoints-1)*(i+1) / numBranch - 1; if (i == numBranch-1) lastIndex = nPoints - 1; // set median vpNode->medians[i] = distances[lastIndex-1]; // next branch childIndex = lastIndex+1; //printf("vpNode->medians[%d] = %f\n",i,vpNode->medians[i]); } free(distances); free(sortList); ///////////////////////////////////////////// // (8) Build VPNode of each child // initialize childs vpNode->child = (VPNode **) malloc(sizeof(VPNode *)*numBranch); if (! vpNode->child) errexit("Err : Not enough memory for allocation!\n\n"); childIndex = 1; for (i=0; i<numBranch; i++) { // last point in this branch lastIndex = 1 + (nPoints-1)*(i+1) / numBranch - 1; if (i == numBranch-1) lastIndex = nPoints - 1; #ifdef _DEBUG printf("from %d to %d\n",childIndex,lastIndex); #endif // set child vpNode->child[i] = buildVPNode(points+childIndex*dimension, dataID+childIndex, lastIndex-childIndex+1, numBranch, dimension); #ifdef _DEBUG if (i != numBranch-1) printf("median : %f\n",vpNode->medians[i]); #endif // next branch childIndex = lastIndex+1; } return vpNode; }
/** Compare two QPoints. Compares two QPoints using their cross product with the lowest y point to determine which point has a lower angle from the x-axis. If the cross product determines that there is a counterclock wise movement, then p1 is considered less than p2. @param p1 The first point to compare. @param p2 The second point to compare. @return true if the points are counter clockerwise. */ bool GrahamScan::operator () (const QPoint& p1, const QPoint& p2) const { int o = ccw(pts[0], p1, p2); return o == 0 ? sqrDist(pts[0], p2) >= sqrDist(pts[0], p1) : o == 2; }
void AutoTriangleMesh<PointType>::ensureEdgeLength(const typename AutoTriangleMesh<PointType>::BasePoint& center,double radius,double minEdgeLength) { double radius2=radius*radius; /* Iterate through all triangles: */ FaceIterator faceIt=BaseMesh::beginFaces(); while(faceIt!=BaseMesh::endFaces()) { /* Check quickly (ha!) if face overlaps area of influence: */ bool overlaps=false; Edge* e=faceIt->getEdge(); do { if(sqrDist(*e->getStart(),center)<=radius2) { overlaps=true; break; } /* Go to next edge: */ e=e->getFaceSucc(); } while(e!=faceIt->getEdge()); if(overlaps) { /* Calculate face's minimum edge length: */ Edge* shortestEdge=0; double shortestEdgeLength2=minEdgeLength*minEdgeLength; Edge* e=faceIt->getEdge(); do { /* Calculate edge's squared length: */ #if 1 double edgeLength2=sqrDist(*e->getStart(),*e->getEnd()); #else /* Calculate normal vectors for edge's vertices: */ float normal1[3],normal2[3]; calcNormal(e->getStart(),normal1); calcNormal(e->getEnd(),normal2); float dist1=0.0f; float dist2=0.0f; float edgeLength2=0.0f; float normal1Length2=0.0f; float normal2Length2=0.0f; for(int i=0;i<3;++i) { float dist=(*e->getEnd())[i]-(*e->getStart())[i]; normal1Length2+=normal1[i]*normal1[i]; normal2Length2+=normal2[i]*normal2[i]; dist1+=dist*normal1[i]; dist2+=dist*normal2[i]; edgeLength2+=dist*dist; } dist1=fabsf(dist1)/sqrtf(normal1Length2); dist2=fabsf(dist2)/sqrtf(normal2Length2); float edgeLength=sqrtf(edgeLength2)+5.0f*(dist1+dist2); edgeLength2=edgeLength*edgeLength; #endif if(shortestEdgeLength2>edgeLength2&&canCollapseEdge(e)) { shortestEdge=e; shortestEdgeLength2=edgeLength2; } /* Go to next edge: */ e=e->getFaceSucc(); } while(e!=faceIt->getEdge()); /* Go to next triangle: */ ++faceIt; /* Check whether the shortest collapsible triangle edge is too short: */ if(shortestEdge!=0) { /* Skip next face if it will be removed by edge collapse: */ if(faceIt==shortestEdge->getOpposite()->getFace()) ++faceIt; /* Collapse shortest collapsible edge: */ collapseEdge(shortestEdge); } } else { /* Go to the next triangle: */ ++faceIt; } } }
void AutoTriangleMesh<PointType>::splitEdge(const typename AutoTriangleMesh<PointType>::EdgeIterator& edge) { /* Get triangle topology: */ Edge* e1=&(*edge); Edge* e2=e1->getFaceSucc(); Edge* e3=e1->getFacePred(); Vertex* v1=e1->getStart(); Vertex* v2=e2->getStart(); Vertex* v3=e3->getStart(); Face* f1=e1->getFace(); assert(e2->getFaceSucc()==e3&&e3->getFacePred()==e2); assert(e2->getFace()==f1); assert(e3->getFace()==f1); assert(f1->getEdge()==e1||f1->getEdge()==e2||f1->getEdge()==e3); Edge* e4=e1->getOpposite(); if(e4!=0) { Edge* e5=e4->getFaceSucc(); Edge* e6=e4->getFacePred(); Vertex* v4=e6->getStart(); Face* f2=e4->getFace(); assert(e5->getFaceSucc()==e6&&e6->getFacePred()==e5); assert(e4->getStart()==v2); assert(e5->getStart()==v1); assert(e5->getFace()==f2); assert(e6->getFace()==f2); assert(f2->getEdge()==e4||f2->getEdge()==e5||f2->getEdge()==e6); /* Don't increase aspect ratio of triangles when splitting: */ double e4Len2=sqrDist(*v1,*v2); double e5Len2=sqrDist(*v1,*v4); double e6Len2=sqrDist(*v2,*v4); if(e4Len2<e5Len2||e4Len2<e6Len2) { /* Split longest edge in neighbouring triangle first: */ if(e5Len2>e6Len2) splitEdge(e5); else splitEdge(e6); /* Re-get triangle topology: */ e4=e1->getOpposite(); e5=e4->getFaceSucc(); e6=e4->getFacePred(); v4=e6->getStart(); f2=e4->getFace(); } /* Create new vertex for edge midpoint: */ Point p=Point::zero(); p.add(*edge->getStart()); p.add(*edge->getEnd()); p.normalize(2); p.index=nextVertexIndex; ++nextVertexIndex; typename BaseMesh::Vertex* nv=newVertex(p); /* Create two quadrilaterals: */ Edge* ne1=BaseMesh::newEdge(); Edge* ne2=BaseMesh::newEdge(); nv->setEdge(ne1); e1->setFaceSucc(ne1); e1->setOpposite(ne2); e2->setFacePred(ne1); e4->setFaceSucc(ne2); e4->setOpposite(ne1); e5->setFacePred(ne2); ne1->set(nv,f1,e1,e2,e4); ne1->sharpness=0; ne2->set(nv,f2,e4,e5,e1); ne2->sharpness=0; f1->setEdge(e1); f2->setEdge(e4); /* Triangulate first quadrilateral: */ Edge* ne3=BaseMesh::newEdge(); Edge* ne4=BaseMesh::newEdge(); Face* nf1=BaseMesh::newFace(); e1->setFaceSucc(ne3); e3->setFacePred(ne3); e2->setFace(nf1); e2->setFaceSucc(ne4); ne1->setFace(nf1); ne1->setFacePred(ne4); ne3->set(nv,f1,e1,e3,ne4); ne3->sharpness=0; ne4->set(v3,nf1,e2,ne1,ne3); ne4->sharpness=0; nf1->setEdge(ne1); /* Triangulate second quadrilateral: */ Edge* ne5=BaseMesh::newEdge(); Edge* ne6=BaseMesh::newEdge(); Face* nf2=BaseMesh::newFace(); e4->setFaceSucc(ne5); e6->setFacePred(ne5); e5->setFace(nf2); e5->setFaceSucc(ne6); ne2->setFace(nf2); ne2->setFacePred(ne6); ne5->set(nv,f2,e4,e6,ne6); ne5->sharpness=0; ne6->set(v4,nf2,e5,ne2,ne5); ne6->sharpness=0; nf2->setEdge(ne2); /* Update version numbers of all involved vertices: */ ++version; v1->version=version; v2->version=version; v3->version=version; v4->version=version; nv->version=version; } else { /* Create new vertex for edge midpoint: */ Point p=Point::zero(); p.add(*edge->getStart()); p.add(*edge->getEnd()); p.normalize(2); p.index=nextVertexIndex; ++nextVertexIndex; typename BaseMesh::Vertex* nv=newVertex(p); /* Create one quadrilateral: */ Edge* ne=BaseMesh::newEdge(); nv->setEdge(ne); e1->setFaceSucc(ne); e2->setFacePred(ne); ne->set(nv,f1,e1,e2,0); ne->sharpness=0; f1->setEdge(e1); /* Triangulate quadrilateral: */ Edge* ne3=BaseMesh::newEdge(); Edge* ne4=BaseMesh::newEdge(); Face* nf1=BaseMesh::newFace(); e1->setFaceSucc(ne3); e3->setFacePred(ne3); e2->setFace(nf1); e2->setFaceSucc(ne4); ne->setFace(nf1); ne->setFacePred(ne4); ne3->set(nv,f1,e1,e3,ne4); ne3->sharpness=0; ne4->set(v3,nf1,e2,ne,ne3); ne4->sharpness=0; nf1->setEdge(ne); /* Update version numbers of all involved vertices: */ ++version; v1->version=version; v2->version=version; v3->version=version; nv->version=version; } }
double QgsPoint::distance( const QgsPoint& other ) const { return sqrt( sqrDist( other ) ); }
bool checkDistance(GEOLIB::Point const& p0, GEOLIB::Point const& p1, double squaredDistance) { return sqrDist(&p0, &p1) < squaredDistance; }
void PointVec::calculateShortestDistance() { size_t i, j; BruteForceClosestPair(*_data_vec, i, j); _sqr_shortest_dist = sqrDist((*_data_vec)[i], (*_data_vec)[j]); }