If approximate is True then only the points from SRTM grid will be
        used, otherwise a basic aproximation of nearby points will be calculated.
11601
def get_elevation(self, latitude, longitude, approximate=None):
        
        if not (self.latitude - self.resolution <= latitude < self.latitude + 1):
            raise Exception('Invalid latitude %s for file %s' % (latitude, self.file_name))
        if not (self.longitude <= longitude < self.longitude + 1 + self.resolution):
            raise Exception('Invalid longitude %s for file %s' % (longitude, self.file_name))

        row, column = self.get_row_and_column(latitude, longitude)

        if approximate:
            return self.approximation(latitude, longitude)
        else:
            return self.get_elevation_from_row_and_column(int(row), int(column))
416736
protected static double calculateLof(LofPoint basePoint, LofDataSet dataSet)
    {
        int countedData = 0;
        double totalAmount = 0.0d;
        for (String targetDataId : basePoint.getkDistanceNeighbor())
        {
            LofPoint targetPoint = dataSet.getDataMap().get(targetDataId);

            totalAmount = totalAmount + (targetPoint.getLrd() / basePoint.getLrd());
            countedData++;
        }

        if (countedData == 0)
        {
            return totalAmount;
        }

        return totalAmount / (countedData);
    }
797976
def coords_on_grid(self, x, y):
        

        if isinstance(x, float):
            x = int(self._round(x))
        if isinstance(y, float):
            y = int(self._round(y))
        if not self._y_coord_down:
            y = self._extents - y
        return x, y
5440
private boolean estimateMotion() {
		// organize the data
		List<Stereo2D3D> data = new ArrayList<>();

		for( PointTrack l : candidates ) {
			LeftTrackInfo info = l.getCookie();
			PointTrack r = info.right;

			Stereo2D3D stereo = info.location;
			// compute normalized image coordinate for track in left and right image
			leftImageToNorm.compute(l.x,l.y,info.location.leftObs);
			rightImageToNorm.compute(r.x,r.y,info.location.rightObs);

			data.add(stereo);
		}

		// Robustly estimate left camera motion
		if( !matcher.process(data) )
			return false;

		Se3_F64 keyToCurr = matcher.getModelParameters();
		keyToCurr.invert(currToKey);

		// mark tracks that are in the inlier set
		int N = matcher.getMatchSet().size();
		for( int i = 0; i < N; i++ ) {
			int index = matcher.getInputIndex(i);
			LeftTrackInfo info = candidates.get(index).getCookie();
			info.lastInlier = tick;
		}

//		System.out.println("Inlier set size: "+N);

		return true;
	}
536010
function(p, opt) {

        opt = opt || {};
        var precision = (opt.precision === undefined) ? this.PRECISION : opt.precision;
        var subdivisions = (opt.subdivisions === undefined) ? this.getSubdivisions({ precision: precision }) : opt.subdivisions;
        // does not use localOpt

        // identify the subdivision that contains the point:
        var investigatedSubdivision;
        var investigatedSubdivisionStartT; // assume that subdivisions are evenly spaced
        var investigatedSubdivisionEndT;
        var distFromStart; // distance of point from start of baseline
        var distFromEnd; // distance of point from end of baseline
        var chordLength; // distance between start and end of the subdivision
        var minSumDist; // lowest observed sum of the two distances
        var n = subdivisions.length;
        var subdivisionSize = (n ? (1 / n) : 0);
        for (var i = 0; i < n; i++) {

            var currentSubdivision = subdivisions[i];

            var startDist = currentSubdivision.start.distance(p);
            var endDist = currentSubdivision.end.distance(p);
            var sumDist = startDist + endDist;

            // check that the point is closest to current subdivision and not any other
            if (!minSumDist || (sumDist < minSumDist)) {
                investigatedSubdivision = currentSubdivision;

                investigatedSubdivisionStartT = i * subdivisionSize;
                investigatedSubdivisionEndT = (i + 1) * subdivisionSize;

                distFromStart = startDist;
                distFromEnd = endDist;

                chordLength = currentSubdivision.start.distance(currentSubdivision.end);

                minSumDist = sumDist;
            }
        }

        var precisionRatio = pow(10, -precision);

        // recursively divide investigated subdivision:
        // until distance between baselinePoint and closest path endpoint is within 10^(-precision)
        // then return the closest endpoint of that final subdivision
        while (true) {

            // check if we have reached at least one required observed precision
            // - calculated as: the difference in distances from point to start and end divided by the distance
            // - note that this function is not monotonic = it doesn't converge stably but has "teeth"
            // - the function decreases while one of the endpoints is fixed but "jumps" whenever we switch
            // - this criterion works well for points lying far away from the curve
            var startPrecisionRatio = (distFromStart ? (abs(distFromStart - distFromEnd) / distFromStart) : 0);
            var endPrecisionRatio = (distFromEnd ? (abs(distFromStart - distFromEnd) / distFromEnd) : 0);
            var hasRequiredPrecision = ((startPrecisionRatio < precisionRatio) || (endPrecisionRatio < precisionRatio));

            // check if we have reached at least one required minimal distance
            // - calculated as: the subdivision chord length multiplied by precisionRatio
            // - calculation is relative so it will work for arbitrarily large/small curves and their subdivisions
            // - this is a backup criterion that works well for points lying "almost at" the curve
            var hasMinimalStartDistance = (distFromStart ? (distFromStart < (chordLength * precisionRatio)) : true);
            var hasMinimalEndDistance = (distFromEnd ? (distFromEnd < (chordLength * precisionRatio)) : true);
            var hasMinimalDistance = (hasMinimalStartDistance || hasMinimalEndDistance);

            // do we stop now?
            if (hasRequiredPrecision || hasMinimalDistance) {
                return ((distFromStart <= distFromEnd) ? investigatedSubdivisionStartT : investigatedSubdivisionEndT);
            }

            // otherwise, set up for next iteration
            var divided = investigatedSubdivision.divide(0.5);
            subdivisionSize /= 2;

            var startDist1 = divided[0].start.distance(p);
            var endDist1 = divided[0].end.distance(p);
            var sumDist1 = startDist1 + endDist1;

            var startDist2 = divided[1].start.distance(p);
            var endDist2 = divided[1].end.distance(p);
            var sumDist2 = startDist2 + endDist2;

            if (sumDist1 <= sumDist2) {
                investigatedSubdivision = divided[0];

                investigatedSubdivisionEndT -= subdivisionSize; // subdivisionSize was already halved

                distFromStart = startDist1;
                distFromEnd = endDist1;

            } else {
                investigatedSubdivision = divided[1];

                investigatedSubdivisionStartT += subdivisionSize; // subdivisionSize was already halved

                distFromStart = startDist2;
                distFromEnd = endDist2;
            }
        }
    }
1222156
def overlaps_point(self, p):
        
        if self.is_empty():
            return False
        return bool(self.top_node.contains_point(p))
969305
public function computeMissing()
    {
        $Missing = [];
        for ($X = 0; $X <= $this->GridSizeX; $X++) {
            for ($Y = 0; $Y <= $this->GridSizeY; $Y++) {
                if ($this->Points[$X][$Y] == UNKNOWN) {
                    $Missing[] = $X . "," . $Y;
                }
            }
        }
        shuffle($Missing);

        foreach ($Missing as $Pos) {
            $Pos = preg_split("/,/", $Pos);
            $X = $Pos[0];
            $Y = $Pos[1];

            if ($this->Points[$X][$Y] == UNKNOWN) {
                $NearestNeighbor = $this->getNearestNeighbor($X, $Y);

                $Value = 0;
                $Points = 0;
                for ($Xi = $X - $NearestNeighbor; $Xi <= $X + $NearestNeighbor; $Xi++) {
                    for ($Yi = $Y - $NearestNeighbor; $Yi <= $Y + $NearestNeighbor; $Yi++) {
                        if ($Xi >= 0
                            && $Yi >= 0
                            && $Xi <= $this->GridSizeX
                            && $Yi <= $this->GridSizeY
                            && $this->Points[$Xi][$Yi] != UNKNOWN
                            && $this->Points[$Xi][$Yi] != IGNORED
                        ) {
                            $Value = $Value + $this->Points[$Xi][$Yi];
                            $Points++;
                        }
                    }
                }

                if ($Points != 0) {
                    $this->Points[$X][$Y] = $Value / $Points;
                }
            }
        }
    }
1200454
private boolean estimateMotion() {
		modelFitData.reset();

		Point2D_F64 normLeft = new Point2D_F64();
		Point2D_F64 normRight = new Point2D_F64();

		// use 0 -> 1 stereo associations to estimate each feature's 3D position
		for( int i = 0; i < quadViews.size; i++ ) {
			QuadView obs = quadViews.get(i);

			// convert old stereo view to normalized coordinates
			leftImageToNorm.compute(obs.v0.x,obs.v0.y,normLeft);
			rightImageToNorm.compute(obs.v1.x,obs.v1.y,normRight);

			// compute 3D location using triangulation
			triangulate.triangulate(normLeft,normRight,leftToRight,obs.X);

			// add to data set for fitting if not at infinity
			if( !Double.isInfinite(obs.X.normSq()) ) {
				Stereo2D3D data = modelFitData.grow();
				leftImageToNorm.compute(obs.v2.x,obs.v2.y,data.leftObs);
				rightImageToNorm.compute(obs.v3.x,obs.v3.y,data.rightObs);
				data.location.set(obs.X);
			}
		}

		// robustly match the data
		if( !matcher.process(modelFitData.toList()) )
			return false;

		Se3_F64 oldToNew = matcher.getModelParameters();

//		System.out.println("matcher rot = "+toString(oldToNew));
		// optionally refine the results
		if( modelRefiner != null ) {
			Se3_F64 found = new Se3_F64();
			if( modelRefiner.fitModel(matcher.getMatchSet(), oldToNew, found) ) {
//				System.out.println("matcher rot = "+toString(found));
				found.invert(newToOld);
			} else {
				oldToNew.invert(newToOld);
//				System.out.println("Fit failed!");
			}
		} else {
			oldToNew.invert(newToOld);
		}

		// compound the just found motion with the previously found motion
		Se3_F64 temp = new Se3_F64();
		newToOld.concat(leftCamToWorld, temp);
		leftCamToWorld.set(temp);

		return true;
	}
606600
private static boolean pointTouchesEnvelope_(Point2D pt_a,
			Envelope2D env_b, double tolerance, ProgressTracker progress_tracker) {
		if (env_b.getHeight() <= tolerance && env_b.getWidth() <= tolerance)
			return false; // when treates as a point, points cannot touch points

		Envelope2D env_b_inflated = new Envelope2D(), env_b_deflated = new Envelope2D();

		env_b_inflated.setCoords(env_b);
		env_b_inflated.inflate(tolerance, tolerance);

		if (!env_b_inflated.contains(pt_a))
			return false;

		if (env_b.getHeight() <= tolerance || env_b.getWidth() <= tolerance) {
			env_b_deflated.setCoords(env_b);

			if (env_b.getHeight() > tolerance)
				env_b_deflated.inflate(0, -tolerance);
			else
				env_b_deflated.inflate(-tolerance, 0);

			if (env_b.getHeight() > tolerance) {
				if (pt_a.y > env_b_deflated.ymin
						&& pt_a.y < env_b_deflated.ymax)
					return false;
			} else {
				if (pt_a.x > env_b_deflated.xmin
						&& pt_a.x < env_b_deflated.xmax)
					return false;
			}

			return true;
		}

		env_b_deflated.setCoords(env_b);
		env_b_deflated.inflate(-tolerance, -tolerance);

		if (env_b_deflated.containsExclusive(pt_a))
			return false;

		return true;
	}
1053222
void filterPointsOnPlaneAtInfinity(List<DMatrixRMaj> homographies_view1_to_view0,
											   List<List<PointIndex2D_F64>> observations,
											   int totalFeatures ) {
		filtered.clear();
		for (int viewIdx = 0; viewIdx < homographies_view1_to_view0.size(); viewIdx++) {
			List<PointIndex2D_F64> filter = new ArrayList<>();
			filtered.add(filter);
			DMatrixRMaj H = homographies_view1_to_view0.get(viewIdx);

			List<PointIndex2D_F64> obs = observations.get(viewIdx);
			for (int i = 0; i < obs.size(); i++) {
				PointIndex2D_F64 p = obs.get(i);

				if( p.index < 0 || p.index >= totalFeatures)
					throw new IllegalArgumentException("Feature index outside of bounds. Must be from 0 to "+(totalFeatures-1));

				GeometryMath_F64.mult(H,p,tmp);
				// Homogenous coordinates are scale invariant. A scale
				// needs to be picked for consistency. I picked the largest x or y value
				double m = Math.max(Math.abs(tmp.x),Math.abs(tmp.y));
				 if( m == 0 ) m = 1;
				tmp.z /= m;

				// See if it's zero or almost zero, meaning it's on the plane at infinity
				if( Math.abs(tmp.z) > infinityThreshold ) {
					filter.add(p);
				}
			}
		}
	}
268644
def extrapolate_points(points, n_points):
    
    points = points[:n_points]
    lat = []
    lon = []
    last = None
    for point in points:
        if last is not None:
            lat.append(last.lat-point.lat)
            lon.append(last.lon-point.lon)
        last = point

    dts = np.mean([p.dt for p in points])
    lons = np.mean(lon)
    lats = np.mean(lat)

    gen_sample = []
    last = points[0]
    for _ in range(n_points):
        point = Point(last.lat+lats, last.lon+lons, None)
        point.dt = dts
        # point.compute_metrics(last)
        gen_sample.append(point)
        last = point

    return gen_sample
803399
def is_point(self):
        

        if self.childCount() == 2:
                if self.child(0).valid_values == float and self.child(1).valid_values == float:
                    return True
        else:
            return False
718744
private void grahamScan() {
    if(points.size() < 3) {
      return;
    }
    Iterator<double[]> iter = points.iterator();
    Stack<double[]> stack = new Stack<>();
    // Start with the first two points on the stack
    final double[] first = iter.next();
    stack.add(first);
    while(iter.hasNext()) {
      double[] n = iter.next();
      if(mdist(first, n) > 0) {
        stack.add(n);
        break;
      }
    }
    while(iter.hasNext()) {
      double[] next = iter.next();
      double[] curr = stack.pop();
      double[] prev = stack.peek();
      while((stack.size() > 1) && (mdist(curr, next) == 0 || !isConvex(prev, curr, next))) {
        curr = stack.pop();
        prev = stack.peek();
      }
      stack.add(curr);
      stack.add(next);
    }
    points = stack;
  }
659101
private static boolean checkVerticesForIntersection_(
			MultiVertexGeometryImpl geom, RasterizedGeometry2D rgeom) {
		// Do a quick raster test for each point. If any point is inside, then
		// there is an intersection.
		int pointCount = geom.getPointCount();
		Point2D pt = new Point2D();
		for (int ipoint = 0; ipoint < pointCount; ipoint++) {
			geom.getXY(ipoint, pt);
			RasterizedGeometry2D.HitType hit = rgeom.queryPointInGeometry(pt.x,
					pt.y);
			if (hit == RasterizedGeometry2D.HitType.Inside) {
				return true;
			}
		}

		return false;
	}
827345
private void closestFarthestPoints(LineSegment2D_F32 a, LineSegment2D_F32 b)
	{
		dist[0] = a.a.distance2(b.a);
		dist[1] = a.a.distance2(b.b);
		dist[2] = a.b.distance2(b.a);
		dist[3] = a.b.distance2(b.b);

		// find the two points which are closest together and save which ones those are
		// for future reference
		farthestIndex = 0;
		float closest = dist[0];
		float farthest = dist[0];
		for( int i = 1; i < 4; i++ ) {
			float d = dist[i];
			if( d < closest ) {
				closest = d;
				closestIndex = i;
			}
			if( d > farthest ) {
				farthest = d;
				farthestIndex = i;
			}
		}
	}
482986
def closest_points(S):
    
    shuffle(S)
    assert len(S) >= 2
    p = S[0]
    q = S[1]
    d = dist(p, q)
    while d > 0:
        r = improve(S, d)
        if r:
            d, p, q = r
        else:
            break
    return p, q
418781
def _approx_equal(x, y, eps):
    
    if x.space != y.space:
        return False

    if x is y:
        return True

    try:
        return x.dist(y) <= eps
    except NotImplementedError:
        try:
            return x == y
        except NotImplementedError:
            return False