pcl::CorrespondencesPtr CloudMapper::LandmarksToCorresponencies(Cloud::Ptr &cloud1, CloudMapper::Landmarks &landmarks1, Cloud::Ptr &cloud2, CloudMapper::Landmarks &landmarks2)
{
pcl::CorrespondencesPtr result(new pcl::Correspondences);
int resultSize = std::min(landmarks1.size(), landmarks2.size());
for (int i = 0; i < resultSize; i++)
{
auto sourcePoint1 = landmarks1.at(i).point;
auto sourcePoint2 = landmarks2.at(i).point;
auto p1IsNotNan = IsNotNAN(sourcePoint1);
auto p2IsNotNan = IsNotNAN(sourcePoint2);
if (p1IsNotNan && p2IsNotNan)
{
CloudMapper::Landmarks::value_type pt1 = FindClosestPoint(cloud1, sourcePoint1);
CloudMapper::Landmarks::value_type pt2 = FindClosestPoint(cloud2, sourcePoint2);
float distance = CalculateDistance(pt1.point, pt2.point);
pcl::Correspondence correspondence(pt1.index, pt2.index, distance);
result->push_back(correspondence);
}
}
return result;
}
bool TriangleEdgesSegmentIntersect(const Vector3f & p1, const Vector3f & p2, const Vector3f p3, const Vector3f & e0, const Vector3f & e1,
Vector3f & intersectionPos, int & edgeIdx, Mesh & mesh, Mesh::FaceHandle & currentFace, Mesh::FaceHandle & previousFace)
{
int r[3] = {0};
//float d1 = DistanceToTrianglePlane(p1, p2, p3, e0);
//float d2 = DistanceToTrianglePlane(p1, p2, p3, e1);
//Vector3f dif = e1 - e0;
//std::cout << '\n' << std::endl;
//PRINTV(e0);
//PRINTV(e1);
//PRINTV(p1);
//PRINTV(p2);
//PRINTV(p3);
Vector3f interPos[3];
r[0] = FindClosestPoint(e0, e1, p3, p1, interPos[0]);
r[1] = FindClosestPoint(e0, e1, p1, p2, interPos[1]);
r[2] = FindClosestPoint(e0, e1, p2, p3, interPos[2]);
if (r[0]+r[1]+r[2] == 3) {
__debugbreak(); //Triangle too small
}
if (r[0]+r[1]+r[2] == 2) {
edgeIdx = 0;
/*for (Mesh::FaceFaceIter ff_it=mesh.ff_begin(currentFace), end=mesh.ff_end(currentFace); ff_it != end; ++ff_it) {
Mesh::FaceHandle ffff = ff_it.handle();
int nodthing=0;
}*/
for (Mesh::FaceFaceIter ff_it=mesh.ff_begin(currentFace), end=mesh.ff_end(currentFace); ff_it != end; ++ff_it, ++edgeIdx) {
// SERIOUS PROBLEM IF THIS HAPPENS
// Never should happen
if (edgeIdx >= 3 || edgeIdx < 0)
__debugbreak();
//Mesh::FaceHandle ffff = ff_it.handle();
if (r[edgeIdx] != 0 && ff_it.handle() != previousFace) break;
}
intersectionPos = interPos[edgeIdx];
return 1;
}
if (r[0]+r[1]+r[2] == 0)
return 0;//__debugbreak(); //Never should happen
edgeIdx = 0*r[0] + 1*r[1] + 2*r[2];
intersectionPos = interPos[edgeIdx];
return 1;
}
void IndividualPointerInteract (PointingEvent *e)
{ int64 last = LastClosestPoint ();
if (last > -1)
SetPointSize (last, 2.0);
int64 closest = FindClosestPoint (e);
if (closest > -1)
SetPointSize (closest, 4.0);
// todo: document this
Vect abs_loc = UnWrangleLoc (PointLocation (closest));
UpdateLabel (e, city_name[closest], abs_loc);
}
void tGisCalc::EstimateRelativeDistanceToLeg( const tMCoord& Start, const tMCoord& End, const tMCoord& Reference, long& Distance, bool& ClosestToStart )
{
tMCoord PerpPoint, ClosestPoint;
PerpPoint = EstimatePerpendicularPoint(Start, End, Reference);
// See if closest point on line is on the line segment bounded by the two endpoints
// If not, use the endpoints to find the nearest distance
ClosestPoint = FindClosestPoint(Start, End, PerpPoint);
ClosestToStart = ( ClosestPoint == Start );
long DeltaLat = ClosestPoint.Y() - Reference.Y();
long DeltaLon = ClosestPoint.X() - Reference.X();
Distance = DoubleToLong(sqrt((double)DeltaLat * DeltaLat + (double)DeltaLon * DeltaLon));
}
/* Clicked mouse */
void Mouse (int button, int state, int x, int y)
{
if (button == GLUT_LEFT_BUTTON)
{
if (state == GLUT_DOWN)
{
mousedown = 1;
xMouse = SCRSIZE * 2.0 * ((float)x/(float)windW - 0.5);
yMouse = -SCRSIZE * 2.0 * ((float)y/(float)windH - 0.5);
}
else if (state == GLUT_UP)
{
FindClosestPoint();
mousedown = 0;
}
}
}
void
avtLocateNodeQuery::Execute(vtkDataSet *ds, const int dom)
{
if (ds == NULL)
{
return;
}
if (!RayIntersectsDataSet(ds))
{
return;
}
avtDataObjectInformation &info = GetInput()->GetInfo();
double dist, isect[3] = { 0., 0., 0.};
int foundNode = -1;
int origNode = -1;
int topodim = info.GetAttributes().GetTopologicalDimension();
int spatdim = info.GetAttributes().GetSpatialDimension();
// Find the cell, intersection point, and distance along the ray.
//
if (ds->GetDataObjectType() != VTK_RECTILINEAR_GRID)
{
if (topodim == 1 && spatdim == 2) // LINES
{
dist = minDist;
foundNode = FindClosestPointOnLine(ds, dist, isect);
}
else
{
int foundCell = LocatorFindCell(ds, dist, isect);
if (foundCell != -1)
{
if (!pickAtts.GetMatSelected())
foundNode = DeterminePickedNode(ds, foundCell, isect);
else
foundNode = FindClosestPoint(ds,foundCell,isect,origNode);
}
}
}
else
{
foundNode = RGridFindNode(ds, dist, isect);
}
if ((foundNode != -1) && (dist < minDist))
{
minDist = dist;
pickAtts.SetPickPoint(isect);
if (!pickAtts.GetMatSelected())
{
vtkDataArray *origNodes =
ds->GetPointData()->GetArray("avtOriginalNodeNumbers");
if (origNodes)
{
int comp = origNodes->GetNumberOfComponents() -1;
foundElement = (int) origNodes->GetComponent(foundNode, comp);
}
else if (info.GetValidity().GetNodesPreserved() &&
info.GetValidity().GetOriginalZonesIntact() &&
(info.GetValidity().GetZonesPreserved() ||
!info.GetValidity().GetPointsWereTransformed()) &&
info.GetAttributes().GetContainsGhostZones()
!= AVT_CREATED_GHOSTS)
{
foundElement = foundNode;
}
// else ... Zones not preserved or we created ghosts, or points
// were transformed, so node id found here is not valid, so don't
// set it.
}
else if (origNode != -1)
{
// MaterialSelection occurred, but we found an original node,
// use that
foundElement = origNode;
}
else if (!info.GetValidity().SubdivisionOccurred())
{
// MaterialSelection occurred without subdivision, can use the
// node id found here.
foundElement = foundNode;
}
// else ... MaterialSelection occurred with subdivision, and the
// original nodes array was not present, so the node id found here
// will not be valid, so don't set it.
pickAtts.SetCellPoint(isect);
pickAtts.SetNodePoint(ds->GetPoint(foundNode));
foundDomain = dom;
} // if node was found
}
void tGisCalc::EstimateRelativeDistanceToLegSquared( long DistShift, const tMCoord& Start, const tMCoord& End, const tMCoord& Reference, long& Distance, tMCoord& ClosestPoint )
{
#if defined(_DEBUG) || defined(CHECKED_BUILD)
bool SpotCheck = false;
#endif
long SDeltaLat = Reference.Y() - Start.Y();
long EDeltaLat = Reference.Y() - End.Y();
long SDeltaLon = Reference.X() - Start.X();
long EDeltaLon = Reference.X() - End.X();
if(sgn(SDeltaLat) == sgn(EDeltaLat) && min(labs(EDeltaLat), labs(SDeltaLat)) > (MAX_ESITMATED_DIST << DistShift))
{
Distance = LONG_MAX;
if(labs(EDeltaLat) < labs(SDeltaLat))
ClosestPoint = End;
else
ClosestPoint = Start;
#if defined(_DEBUG) || defined(CHECKED_BUILD)
SpotCheck = true;
#else
return;
#endif
}
if(sgn(SDeltaLon) == sgn(EDeltaLon) && min(labs(EDeltaLon), labs(SDeltaLon)) > (MAX_ESITMATED_DIST << DistShift))
{
Distance = LONG_MAX;
if(labs(EDeltaLon) < labs(SDeltaLon))
ClosestPoint = End;
else
ClosestPoint = Start;
#if defined(_DEBUG) || defined(CHECKED_BUILD)
SpotCheck = true;
#else
return;
#endif
}
/*
//~~~~~~~~~~~~~~~~
// AC - If we are off both ends, we don't need to calculate the perpendicular point...
if(((Reference.Y() < Start.Y() && Reference.Y() < End.Y()) || (Reference.Y() > Start.Y() && Reference.Y() > End.Y())) &&
((Reference.X() < Start.X() && Reference.X() < End.X()) || (Reference.X() > Start.X() && Reference.X() > End.X())))
{
// See if closest point on line is on the line segment bounded by the two endpoints
// If not, use the endpoints to find the nearest distance
ClosestPoint = FindClosestPoint(Start, End, Reference);
}
else*/
{
tMCoord PerpPoint;
PerpPoint = EstimatePerpendicularPoint(Start, End, Reference);
// See if closest point on line is on the line segment bounded by the two endpoints
// If not, use the endpoints to find the nearest distance
ClosestPoint = FindClosestPoint(Start, End, PerpPoint);
}
long DeltaLat = labs(ClosestPoint.Y() - Reference.Y()) >> DistShift;
long DeltaLon = labs(ClosestPoint.X() - Reference.X()) >> DistShift;
if(DeltaLat < MAX_ESITMATED_DIST && DeltaLon < MAX_ESITMATED_DIST)
Distance = DeltaLat * DeltaLat + DeltaLon * DeltaLon;
else
Distance = LONG_MAX;
#if defined(_DEBUG) || defined(CHECKED_BUILD)
assert(!SpotCheck || Distance == LONG_MAX);
#endif
}
void tGisCalc::EstimateDistanceSquaredToSegment( const tVector2l& End1, const tVector2l& End2, long MaxDist, long& DistanceSquared, tVector2l& ClosestPoint )
{
// If we are too far away, we don't need to calculate anything...
if((End1.x < -MaxDist && End2.x < -MaxDist)
|| (End1.x > MaxDist && End2.x > MaxDist)
|| (End1.y < -MaxDist && End2.y < -MaxDist)
|| (End1.y > MaxDist && End2.y > MaxDist))
{
DistanceSquared = MaxDist * MaxDist;
ClosestPoint = End1;
assert(DistanceSquared >= 0);
return;
}
// Check for an undefined slope
if(End1.x == End2.x)
{
if(sgn(End1.y) != sgn(End2.y))
{
ClosestPoint.y = 0;
}
else
{
ClosestPoint.y = End1.y;
if(labs(End2.y) < labs(ClosestPoint.y))
ClosestPoint.y = End2.y;
}
ClosestPoint.x = End1.x;
DistanceSquared = (ClosestPoint.x * ClosestPoint.x + ClosestPoint.y * ClosestPoint.y);
assert(DistanceSquared >= 0);
}
else
{
// First, assume the best possible distance to see if it beats our MaxDist
if(sgn(End1.x) == sgn(End2.x))
{
if(labs(End1.x) > MaxDist || labs(End2.x) > MaxDist)
{
DistanceSquared = MaxDist * MaxDist;
return;
}
}
if(sgn(End1.y) == sgn(End2.y))
{
if(labs(End1.y) > MaxDist || labs(End2.y) > MaxDist)
{
DistanceSquared = MaxDist * MaxDist;
return;
}
}
// Write out the equation for two lines, the line segment, and a line perpendicular to the semgent. y=mx+b and y'=-x'/m Set y'=y and x'=x and solve for x.
float m, ReturnY;
float dx;
dx = (float)(End1.x - End2.x);
m = (End1.y - End2.y) / dx;
ReturnY = (End1.y - End1.x * m) / (1.0F + m * m);
ClosestPoint.y = FloatToLong(ReturnY);
ClosestPoint.x = FloatToLong(-m * ReturnY);
ClosestPoint = FindClosestPoint(End1, End2, ClosestPoint);
DistanceSquared = (ClosestPoint.x * ClosestPoint.x + ClosestPoint.y * ClosestPoint.y);
assert(DistanceSquared >= 0);
}
}
double Cluster::DistanceMetric(Cluster* c1){
Cluster* c2 = new Cluster(center, points);
Point* p1 = FindClosestPoint(&c1->points);
Point* p2 = c2->FindClosestPoint(&points);
return pntfnc.distance(p1, p2);
}
