double Vertex::interpolateZ(const Coordinate &p, const Coordinate &p0,
const Coordinate &p1)
{
double segLen = p0.distance(p1);
double ptLen = p.distance(p0);
double dz = p1.z - p0.z;
double pz = p0.z + dz * (ptLen / segLen);
return pz;
}
long WayAverager::_moveToLine(long ni, double nWeight, const LineString* ls, double lWeight,
int w1OrW2)
{
shared_ptr<Node> n = _map.getNode(ni);
Coordinate c = _moveToLineAsCoordinate(ni, nWeight, ls, lWeight);
Meters d = c.distance(n->toCoordinate());
if (w1OrW2 == 1)
{
_sumMovement1 += d;
_moveCount1++;
_maxMovement1 = max(_maxMovement1, d);
}
else
{
_sumMovement2 += d;
_moveCount2++;
_maxMovement2 = max(_maxMovement2, d);
}
n->setX(c.x);
n->setY(c.y);
return ni;
}
double
FacetSequence::distance(const FacetSequence& facetSeq) const
{
bool isPointThis = isPoint();
bool isPointOther = facetSeq.isPoint();
double distance;
if(isPointThis && isPointOther) {
Coordinate pt = pts->getAt(start);
Coordinate seqPt = facetSeq.pts->getAt(facetSeq.start);
distance = pt.distance(seqPt);
}
else if(isPointThis) {
Coordinate pt = pts->getAt(start);
distance = computeDistancePointLine(pt, facetSeq, nullptr);
}
else if(isPointOther) {
Coordinate seqPt = facetSeq.pts->getAt(facetSeq.start);
distance = computeDistancePointLine(seqPt, *this, nullptr);
}
else {
distance = computeDistanceLineLine(facetSeq, nullptr);
}
return distance;
}
bool
Geometry::equal(const Coordinate& a, const Coordinate& b,
double tolerance) const
{
if (tolerance==0)
{
return a == b; // 2D only !!!
}
//double dist=a.distance(b);
return a.distance(b)<=tolerance;
}
WayLocation::WayLocation(ConstOsmMapPtr map, ConstWayPtr way, double distance) :
_map(map)
{
double d = 0.0;
_segmentIndex = -1;
_segmentFraction = -1;
_way = way;
double length = ElementConverter(map).convertToLineString(way)->getLength();
if (distance <= 0)
{
_segmentIndex = 0.0;
_segmentFraction = 0;
}
else if (distance >= length)
{
_segmentIndex = _way->getNodeCount() - 1;
_segmentFraction = 0.0;
}
else
{
Coordinate last = _map->getNode(way->getNodeId(0))->toCoordinate();
_segmentIndex = way->getNodeCount() - 1;
_segmentFraction = 0;
for (size_t i = 1; i < way->getNodeCount(); i++)
{
ConstNodePtr n = _map->getNode(_way->getNodeId(i));
Coordinate next = n->toCoordinate();
double delta = next.distance(last);
last = next;
if (d <= distance && d + delta > distance)
{
_segmentIndex = i - 1;
_segmentFraction = (distance - d) / delta;
// this can sometimes happen due to rounding errors.
if (_segmentFraction >= 1.0)
{
_segmentFraction = 0.0;
_segmentIndex++;
}
_way = way;
break;
}
d += delta;
}
}
assert(_segmentFraction < 1.0);
assert((size_t)_segmentIndex <= _way->getNodeCount() - 1);
}
Coordinate WayHeading::calculateVector(const Coordinate& c1, const Coordinate& c2)
{
Coordinate result;
result.x = c2.x - c1.x;
result.y = c2.y - c1.y;
double mag = result.distance(Coordinate(0, 0));
result.x /= mag;
result.y /= mag;
return result;
}
void runOffsetTest(string const& inputWKT, string const& testPtWKT,
double offsetDistance, string const& expectedPtWKT)
{
GeomPtr input(reader.read(inputWKT));
GeomPtr testPoint(reader.read(testPtWKT));
GeomPtr expectedPoint(reader.read(expectedPtWKT));
const Coordinate* testPt = testPoint->getCoordinate();
const Coordinate* expectedPt = expectedPoint->getCoordinate();
Coordinate offsetPt = extractOffsetAt(input.get(), *testPt, offsetDistance);
bool isOk = offsetPt.distance(*expectedPt) < TOLERANCE_DIST;
if (! isOk)
cout << "Expected = " << *expectedPoint << " Actual = " << offsetPt << endl;
ensure(isOk);
}
Coordinate WayHeading::calculateVector(const WayLocation& loc, Meters delta)
{
Coordinate sc = loc.move(-delta).getCoordinate();
Coordinate ec = loc.move(delta).getCoordinate();
Coordinate result;
result.x = ec.x - sc.x;
result.y = ec.y - sc.y;
double mag = result.distance(Coordinate(0, 0));
result.x /= mag;
result.y /= mag;
return result;
}
Meters WayLocation::calculateDistanceOnWay() const
{
//LOG_TRACE("Calculating distance on way...");
Meters result = 0.0;
Coordinate last = _map->getNode(_way->getNodeId(0))->toCoordinate();
for (int i = 1; i < (int)_way->getNodeCount() && i <= _segmentIndex; i++)
{
ConstNodePtr n = _map->getNode(_way->getNodeId(i));
Coordinate next = n->toCoordinate();
result += next.distance(last);
last = next;
}
if (_segmentIndex < (int)_way->getNodeCount() - 1)
{
ConstNodePtr n = _map->getNode(_way->getNodeId(_segmentIndex + 1));
Coordinate next = n->toCoordinate();
Meters d = next.distance(last);
result += d * _segmentFraction;
}
return result;
}
/* private */
void
OffsetCurveBuilder::addMitreJoin(const geom::Coordinate& p,
const geom::LineSegment& offset0,
const geom::LineSegment& offset1,
double distance)
{
bool isMitreWithinLimit = true;
Coordinate intPt;
/**
* This computation is unstable if the offset segments
* are nearly collinear.
* Howver, this situation should have been eliminated earlier
* by the check for whether the offset segment endpoints are
* almost coincident
*/
try
{
HCoordinate::intersection(offset0.p0, offset0.p1,
offset1.p0, offset1.p1,
intPt);
double mitreRatio = distance <= 0.0 ? 1.0
: intPt.distance(p) / fabs(distance);
if (mitreRatio > bufParams.getMitreLimit())
isMitreWithinLimit = false;
}
catch (const NotRepresentableException& e)
{
::geos::ignore_unused_variable_warning(e);
intPt = Coordinate(0,0);
isMitreWithinLimit = false;
}
if (isMitreWithinLimit)
{
vertexList->addPt(intPt);
}
else
{
addLimitedMitreJoin(offset0, offset1, distance,
bufParams.getMitreLimit());
//addBevelJoin(offset0, offset1);
}
}
/*private*/
void
InteriorPointLine::add(const Coordinate& point)
{
double dist=point.distance(centroid);
#if GEOS_DEBUG
std::cerr << "point " << point << " dist " << dist << ", minDistance " << minDistance << std::endl;
#endif
if (!hasInterior || dist<minDistance) {
interiorPoint=point;
#if GEOS_DEBUG
std::cerr << " is new InteriorPoint" << std::endl;
#endif
minDistance=dist;
hasInterior=true;
}
}
double FacetSequence::distance(const FacetSequence & facetSeq) const {
bool isPointThis = isPoint();
bool isPointOther = facetSeq.isPoint();
if (isPointThis && isPointOther) {
Coordinate pt = pts->getAt(start);
Coordinate seqPt = facetSeq.pts->getAt(facetSeq.start);
return pt.distance(seqPt);
} else if (isPointThis) {
Coordinate pt = pts->getAt(start);
return computePointLineDistance(pt, facetSeq);
} else if (isPointOther) {
Coordinate seqPt = facetSeq.pts->getAt(facetSeq.start);
return computePointLineDistance(seqPt, *this);
}
return computeLineLineDistance(facetSeq);
}
virtual void visit(const ConstElementPtr& e)
{
if (e->getElementType() == ElementType::Way)
{
const ConstWayPtr& w = boost::dynamic_pointer_cast<const Way>(e);
vector<long> nodes = w->getNodeIds();
if (nodes[0] != nodes[nodes.size() - 1])
{
nodes.push_back(nodes[0]);
}
Coordinate last = _map.getNode(nodes[0])->toCoordinate();
for (size_t i = 1; i < nodes.size(); i++)
{
Coordinate c = _map.getNode(nodes[i])->toCoordinate();
double distance = c.distance(last);
double theta = atan2(c.y - last.y, c.x - last.x);
_h.addAngle(theta, distance);
last = c;
}
}
}
void MapPointsLayer::onViewChanged(const Coordinate& a, const Coordinate& b)
{
if(m_hasActivePoint)
{
// don't draw points
return;
}
// TODO: update point on demand every half a second or so
// will not significantly affect UX but will improve performance
// Don't forget to resize all children of m_points
Coordinate center = (a + b)/2;
float radius = center.distance(a);
std::list<MapPoint*> list;
MapViewLayer::Context.PointsLoader->findAllPointsInRadius(center, radius, list);
m_points->removeAllChildren();
for(auto p : list)
{
addPoint(p);
}
}
WayLocation WayLocation::move(Meters distance) const
{
WayLocation result(*this);
Coordinate last = result.getCoordinate();
// This odd statement avoid us adding irrelevantly small distances.
while (1 + distance > 1)
{
// if we're at the end of the way
if (result.isLast())
{
return result;
}
ConstNodePtr n = _map->getNode(_way->getNodeId(result.getSegmentIndex() + 1));
Coordinate next = n->toCoordinate();
double delta = last.distance(next);
// if the next node is too far
if (distance - delta < 0)
{
// figure out what distance we need to move along the segment.
Coordinate lastSegment = _map->getNode(_way->getNodeId(result.getSegmentIndex()))->
toCoordinate();
double segmentLength = lastSegment.distance(next);
result._segmentFraction += (distance / segmentLength);
// this can happen due to floating point errors when the new location is very close to a
// node.
if (result._segmentFraction >= 1.0)
{
result._segmentFraction = 0.0;
result._segmentIndex = result._segmentIndex + 1;
}
distance = 0;
}
// if we need to go past the next node
else
{
distance -= delta;
last = next;
result._segmentIndex = result.getSegmentIndex() + 1;
result._segmentFraction = 0.0;
}
}
// This odd statement avoid us subtracting irrelevantly small distances.
while (1 + distance < 1)
{
// if we're at the end of the way
if (result.isFirst())
{
return result;
}
if (result._segmentFraction > 0)
{
Coordinate next = _map->getNode(_way->getNodeId(result.getSegmentIndex()))->toCoordinate();
Meters delta = last.distance(next);
if (distance + delta > 0)
{
// figure out what distance we need to move along the segment.
Coordinate last = _map->getNode(_way->getNodeId(result._segmentIndex + 1))->toCoordinate();
double segmentLength = last.distance(next);
result._segmentFraction += (distance / segmentLength);
// this can happen due to floating point errors when the new location is very close to a
// node.
if (result._segmentFraction >= 1.0)
{
result._segmentFraction = 0.0;
result._segmentIndex = result._segmentIndex + 1;
}
distance = 0;
}
else
{
result._segmentFraction = 0;
distance += delta;
last = next;
}
}
else
{
Coordinate next = _map->getNode(_way->getNodeId(result.getSegmentIndex() - 1))->
toCoordinate();
Meters delta = last.distance(next);
if (distance + delta > 0)
{
// figure out what distance we need to move along the segment.
Coordinate last = _map->getNode(_way->getNodeId(result.getSegmentIndex()))->toCoordinate();
double segmentLength = last.distance(next);
result._segmentFraction = 1.0 + (distance / segmentLength);
// if we're suffering from a floating point issue.
if (result._segmentFraction >= 1.0)
{
// make sure the floating point issue is within the expected bounds.
assert(result._segmentFraction < 1.0 + 1e-14);
result._segmentFraction = 0;
}
else
{
result._segmentIndex = result.getSegmentIndex() - 1;
}
distance = 0;
}
else
{
result._segmentIndex--;
distance += delta;
last = next;
}
}
}
assert(result._segmentFraction < 1.0 && result._segmentFraction >= 0);
assert(result._segmentIndex >= 0 && result._segmentIndex < (int)result.getWay()->getNodeCount());
return result;
}
// -----------------------------------------------------------------------------
//
Process::Process(const Configuration & first,
const Configuration & second,
const double rate,
const std::vector<int> & basis_sites,
const std::vector<int> & move_origins,
const std::vector<Coordinate> & move_vectors,
const int process_number) :
process_number_(process_number),
range_(1),
rate_(rate),
cutoff_(0.0),
sites_(0),
affected_indices_(0),
basis_sites_(basis_sites),
id_moves_(0)
{
// Get a handle to the coordinates and elements.
const std::vector<Coordinate> & coords = first.coordinates();
// Get the first coordinate out to calculate the distance against.
const Coordinate origin = coords[0];
// Transform the configurations into match lists.
for (size_t i = 0; i < first.elements().size(); ++i)
{
// Get the types as integers.
const int first_type = first.types()[i];
const int second_type = second.types()[i];
// Calculate the distance.
const Coordinate coordinate = coords[i];
const double distance = coordinate.distance(origin);
// Save the cutoff.
if (distance > cutoff_)
{
cutoff_ = distance;
}
// Calculate the range based on the coordinates, such that all
// needed coordinates are guarranteed to be included.
const double x = coordinate.x();
const int cmp_x = static_cast<int>( ( x < 0.0 ) ? (-1.0*x)+0.99999 : x );
range_ = std::max(cmp_x, range_);
const double y = coordinate.y();
const int cmp_y = static_cast<int>( ( y < 0.0 ) ? (-1.0*y)+0.99999 : y );
range_ = std::max(cmp_y, range_);
const double z = coordinate.z();
const int cmp_z = static_cast<int>( ( z < 0.0 ) ? (-1.0*z)+0.99999 : z );
range_ = std::max(cmp_z, range_);
// Set up the match list.
MinimalMatchListEntry m;
m.match_type = first_type;
m.update_type = second_type;
m.distance = distance;
m.coordinate = coordinate;
m.index = -1;
m.has_move_coordinate = false;
m.move_coordinate = Coordinate(0.0, 0.0, 0.0);
minimal_match_list_.push_back(m);
// If the first and second type differ increase the length of the
// affected_sites list accordingly.
if (first_type != second_type)
{
affected_indices_.push_back(0);
}
}
// Loop over the move vector origins and place the move vectors
// on the match list entries before sorting.
for (size_t i = 0; i < move_origins.size(); ++i)
{
const int move_origin = move_origins[i];
minimal_match_list_[move_origin].move_coordinate = move_vectors[i];
minimal_match_list_[move_origin].has_move_coordinate = true;
}
// Sort the match list.
std::sort(minimal_match_list_.begin(), minimal_match_list_.end());
// Find out which index in the match list each move vector
// points to.
for (size_t i = 0; i < minimal_match_list_.size(); ++i)
{
if (minimal_match_list_[i].has_move_coordinate)
{
// If this move vector is different from zero we go on and try to find
// which index in the sorted match list it points to.
// Get the move vector out.
const Coordinate & move_vector = minimal_match_list_[i].move_coordinate;
// Setup the destination coordinate.
const Coordinate destination = minimal_match_list_[i].coordinate + move_vector;
for (size_t j = 0; j < minimal_match_list_.size(); ++j)
{
// We can only move to a coordinate which also has a
// move coordinate.
if (minimal_match_list_[j].has_move_coordinate && (j != i) )
{
// If the difference is small enough we have a match.
const Coordinate diff = minimal_match_list_[j].coordinate - destination;
if (diff.norm() < 1.0e-6)
{
id_moves_.push_back(std::pair<int,int>(i,j));
break;
}
}
}
}
}
}
bool equals(const Coordinate& p0, const Coordinate& p1,
double distanceTolerance)
{
return p0.distance(p1) <= distanceTolerance;
}
