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]);
}
