/*
* Copyright (C) 2014 Steve Ratcliffe
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 3 or
* version 2 as published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
package test.check;
import java.util.ArrayList;
import java.util.BitSet;
import java.util.List;
import java.util.Map;
import uk.me.parabola.imgfmt.app.Coord;
import uk.me.parabola.imgfmt.app.Label;
import uk.me.parabola.imgfmt.app.net.NODHeader;
import test.elements.Line;
import test.files.Nod2Record;
import test.files.NodeLocator;
import test.files.RoadData;
import test.files.RouteArc;
import test.files.RouteCenter;
import test.files.RouteNode;
import test.files.Segment;
import static test.files.RouteArc.directionFromDegrees;
/**
* Check a NOD file for consistency and print out various things that might be useful in debugging.
*
* @author Steve Ratcliffe
*/
public class NodCheck
extends CommonCheck
{
protected void print
() {
openLbl
();
openTre
();
openNet
();
openRgn
();
openNod
();
reader.
position(0);
List<RouteCenter
> routeCenters = nod.
readNodeCenters();
initRoads
();
checkNod2
();
check
(routeCenters
);
}
private void checkNod2
() {
int end =
((NODHeader
) nod.
getHeader()).
getRoadSection().
getSize();
int pos =
0;
do {
Nod2Record nod2 = nod.
getNod2(pos
);
byte[] bitStream = nod2.
getBitStream();
RoadData road = nod2.
getRoadData();
if (road ==
null) {
error
("could not find road for nod2=%x", pos
);
pos =
(int) nod2.
getNext();
continue;
}
BitSet bset =
BitSet.
valueOf(bitStream
);
int nInStream = nod2.
getBitStreamLen();
info
("Road %x: node=%x, %s", road.
getOffset(), nod2.
getNode().
getOffset(),
bitString
(bset, nInStream
));
matchRoadsNodes
(road, nod2
);
pos =
(int) nod2.
getNext();
} while (pos
< end
&& pos
!=
0);
}
private String bitString
(BitSet bset,
int size
) {
StringBuilder sb =
new StringBuilder();
for (int i =
0; i
< size
; i++
)
sb.
append(bset.
get(i
) ? "1" :
"0");
return sb.
toString();
}
/**
* Match up routing nodes to points on the road.
*
* A road consists of one or more line segments. Each line has a number of points.
* Some of those points have the 'node-flag' set. Some of those flagged points are
* routing nodes and some are only for house number ranges (and perhaps other reasons).
*
* The bitstream in nod 2 tells you which kind of node you have. The bit is set
* to 1 for a routing node and zero otherwise.
*
* Note that the nod 2 bitstream also covers the first and last points of a road. Its best to
* think of the first and last points in a road having the node-flag set implicitly.
*
* @param road The road to examine.
* @param nod2 The NOD 2 record that the road refers to. A road can only point to one
* NOD 2 record.
*/
private void matchRoadsNodes
(RoadData road, Nod2Record nod2
) {
BitSet bset =
BitSet.
valueOf(nod2.
getBitStream());
List<RouteNode
> routeNodes = getRouteNodes
(road, nod2.
getNode());
if (routeNodes.
size() != bset.
cardinality()) {
error
("number of route nodes (%d) not equal to set bits %s", routeNodes.
size(),
bset.
toString());
// At the moment, don't try to set the nodes up
return;
}
List<Segment> segments = road.
getSegments();
int nseg = segments.
size();
int rnod =
0;
int rcount =
0;
int npoint =
0;
double length =
0;
Coord lastCoord =
null;
for (int s =
0; s
< nseg
; s++
) {
Segment seg = segments.
get(s
);
Line line = seg.
getLine();
List<Coord
> coords = line.
getCoords();
int ncoord = coords.
size();
for (int c =
0; c
< ncoord
; c++
) {
Coord co = coords.
get(c
);
if (lastCoord
!=
null)
length += lastCoord.
distance(co
);
lastCoord = co
;
// This counts as a road node if it is the first or last in the road, or it has
// the node-flag set.
boolean last = c == ncoord -
1 && s == nseg -
1;
if (co.
getId() > 0 || npoint ==
0 || last
) {
if (s
> 0 && c ==
0)
continue;
// See if this road node is also a routing node
if (bset.
get(rnod
)) {
co.
setTreatAsNode(true); // Use this flag to mark as a real routing node
RouteNode node = routeNodes.
get(rcount
);
if (!equalCoord
(node.
getCoord(), co
))
error
("coords dont match n=%s, c=%s", node.
getCoord(), co
);
NodeLocator locator =
new NodeLocator
(node
);
locator.
setIndex(rcount
);
locator.
setSegmentNumber(s
);
locator.
setCoordNumber(c
);
locator.
setLength((int) Math.
round(length
));
road.
addNodeLocation(locator
);
trace
(" Node:%x %s", node.
getOffset(), fmtCoord
(co
));
rcount++
;
}
rnod++
;
}
npoint++
;
}
}
if (rcount
< routeNodes.
size()) {
for (int i = rcount
; i
< routeNodes.
size(); i++
) {
RouteNode node = routeNodes.
get(i
);
StringBuilder sb =
new StringBuilder();
for (Segment seg : road.
getSegments()) {
List<Coord
> coords = seg.
getLine().
getCoords();
sb.
append(coords
);
}
error
("missing node %x %s %s", node.
getOffset(), node.
getCoord(),
sb.
toString());
}
}
}
/**
* Are coords equal to withing a margin of error.
*
* Often the node coords are one out compared to the corresponding coordinate. Possible
* that we are missing something, as you might expect them to be the same.
*
* @param c1 First coordinate to compare.
* @param c2 Second coordinate to compare.
* @return True if they are equal to within one unit.
*/
public boolean equalCoord
(Coord c1, Coord c2
) {
if (Math.
abs(c1.
getLatitude() - c2.
getLatitude()) < 2
&& Math.
abs(c1.
getLongitude() - c2.
getLongitude()) < 2)
return true;
else
return false;
}
/**
* Get a list of routing nodes for a road. We do this by walking along the forward arcs
* for the road.
*
* We assume that the first arc in the forward direction is always a link to the next
* node in this routine. That is believed to be how it is meant to be.
*
* @param road The road to examine.
* @param first The first (routing) node on the road as taken from nod 2.
* @return A list of all the routing nodes found for the road. They are in order from
* the first.
*/
private List<RouteNode
> getRouteNodes
(RoadData road, RouteNode first
) {
List<RouteNode
> nodes =
new ArrayList<>();
nodes.
add(first
);
for (int i =
0; i
< road.
getNod2().
getBitStreamLen(); i++
) {
for (RouteArc arc : first.
getArcs()) {
if (arc.
isSign() && arc.
getNetOffset() == road.
getOffset()) {
RouteNode next = nod.
getNode(arc.
getLink());
nodes.
add(next
);
first = next
;
break;
}
}
}
return nodes
;
}
private void check
(List<RouteCenter
> routeCenters
) {
for (RouteCenter center : routeCenters
) {
info
("Route center %d: table at %#x, loc(%s)", center.
getIndex(), center.
getOffset(),
fmtCoord
(center.
getCoord()));
checkCenter
(center
);
}
}
private void checkCenter
(RouteCenter center
) {
RouteNode last =
null;
for (RouteNode node : center.
nodes().
values()) {
checkNode
(node, last
);
last = node
;
if (node.
isBad())
continue;
checkArcs
(node
);
}
}
private void checkNode
(RouteNode node, RouteNode previous
) {
// Check for gap or overlap with previous
if (previous
!=
null && node.
getOffset() > previous.
getNextStart()) {
error
("Node:%x: gap %d from last (%x)", node.
getOffset(),
node.
getOffset() - previous.
getNextStart(),
previous.
getNextStart());
} else if (previous
!=
null && node.
getOffset() < previous.
getNextStart()) {
error
("Node:%x: overlap with last (%x)", node.
getOffset(), previous.
getNextStart());
}
// Display a summary
info
(" %x:Node%s: (%s) flags:'%s', arcs=%d destclass=%d", node.
getOffset(), node.
isProblem() ? "!" :
"",
fmtCoord
(node.
getCoord()), node.
flagString(), node.
getArcs().
size(),
node.
getDestclass());
}
/**
* A node has zero or more arcs to other nodes.
*
* @param node The starting node.
*/
private void checkArcs
(RouteNode node
) {
int maxRoadclass =
0;
int count =
0;
for (RouteArc arc : node.
getArcs()) {
if (arc.
isBad()) {
info
(" Arc %d[%x]: BAD", count, arc.
getOffset());
count++
;
continue;
}
if (arc.
getRoadClass() > maxRoadclass
)
maxRoadclass = arc.
getRoadClass();
//findLinesForArc(node, arc);
// Display info about the arc
Integer netOffset = arc.
getNetOffset();
RoadData rd = net.
getRoad(netOffset
);
assert rd
!=
null :
String.
format("net %x", netOffset
);
arc.
setRoad(rd
);
Label label = rd.
getLabel(0);
String roadInfo =
String.
format("rd=%x:%s:cls=%d/%d", arc.
getNetOffset(),
Label.
stripGarminCodes(label.
getText()),
arc.
getRoadClass(), arc.
getSpeed() );
info
(" Arc %d[%x]: %s, [%s] ->%x%s, %dm %s%s",
count, arc.
getOffset(), arc.
alt6String(), roadInfo,
arc.
getLink(), arc.
isLongLink() ? "(LL)" :
"",
nod.
rawToMeters(arc.
getDistance()), arc.
hasDirection()? String.
format("%.2fdeg", arc.
getInitialDegrees()):
"ND",
arc.
isLast()? " LAST":
"");
// Check that the class/speed from the arc, matches the values from NET 2.
Nod2Record nod2Record = rd.
getNod2();
if (arc.
getRoadClass() != nod2Record.
getRoadClass()) {
error
("road class from arc %d, does not match from net2 %d",
arc.
getRoadClass(), nod2Record.
getRoadClass());
}
if (arc.
getSpeed() != nod2Record.
getSpeed()) {
error
("road class from arc %d, does not match from net2 %d",
arc.
getSpeed(), nod2Record.
getSpeed());
}
// Check the target node, make sure it exists and is valid
int link = arc.
getLink();
RouteNode other = nod.
getNode(link
);
if (other ==
null) {
error
("Node:%x/%d: node target not read %x", node.
getOffset(), arc.
getIndex(), link
);
} else if (other.
isBad()) {
error
("Node:%x/%d: node target bad %x", node.
getOffset(), arc.
getIndex(), link
);
} else {
arc.
setTargetNode(other
);
// Compare actual distance and direction with this other node from this arc
compareActual
(arc, node, other
);
checkReverse
(arc, other
);
checkCurve
(arc, other
);
checkRoadClass
(arc, other
);
}
count++
;
}
if (maxRoadclass
!= node.
getDestclass()) {
error
("node:%x: class on node %d, max destclass of arcs %d", node.
getOffset(),
node.
getDestclass(), maxRoadclass
);
}
}
/**
* Print out road class information and check it is correct.
* Not sure about the road class understanding, often the arc dest class is zero
* when you would not expect it to be.
*
* Investigating.
*
* @param arc The arc to check.
* @param target The arc target node.
*/
private void checkRoadClass
(RouteArc arc, RouteNode target
) {
RouteNode node = arc.
getNode();
trace
(" road class: this=%d arcdst=%d this-node-dst=%d target-node-dst=%d",
arc.
getRoadClass(), arc.
getDestclass(), node.
getDestclass(), target.
getDestclass());
// The destination class on the arc is often zero when you would not expect it to be.
}
private void checkCurve
(RouteArc arc, RouteNode other
) {
if (!arc.
hasCurve())
return;
Coord c1 = arc.
getNode().
getCoord();
Coord c2 = other.
getCoord();
double actualStraighLine = c1.
bearingTo(c2
);
int cdir = arc.
getCurveDirection();
int actualCDir = RouteArc.
directionFromDegrees(actualStraighLine
);
trace
(" curve A=%x[%x,%x->%x], B=%x direct=%.0fdeg[%x], dstc=%d, len=%dm", arc.
getCurveA(),
(arc.
getCurveA() & 0xe0
)>>5, arc.
getCurveA() & 0x1f,
cdir,
arc.
getCurveB(),
actualStraighLine, actualCDir,
arc.
getDestclass(),
nod.
rawToMeters(arc.
getDistance()));
int diff =
Math.
abs(cdir - actualCDir
);
if (diff
> 128)
diff -=
256;
if (diff
> 8)
error
("Node:%x/%d: curve direct bearing %x, expected %x", arc.
getNode().
getOffset(), arc.
getIndex(),
cdir, actualCDir
);
if (arc.
hasCurve()) {
int direct= nod.
metersToRaw(
arc.
getNode().
getCoord().
distance(other.
getCoord()));
int path = arc.
getDistance();
double v =
30.0;
int calcRatio =
(int) Math.
round(v
* direct / path
) & 0x1f
;
if (calcRatio
> 26)
calcRatio =
0;
int hi =
(int) Math.
round(v
* (direct +
1) / path
);
int lo =
(int) Math.
round(v
* (direct -
1) / path
);
//test("orig hi%x lo%x calc%x", hi, lo, calcRatio);
int ratio =
(arc.
getCurveA() & 0xe0
) >> 5;
if (ratio
> 0) {
// Compressed/limited range of ratios, so adjust.
calcRatio = calcRatio /
2 -
8;
if (calcRatio == -
8)
calcRatio =
7;
lo = lo /
2 -
8;
hi = hi /
2 -
8;
//test("hi%x lo%x calc%x", hi, lo, calcRatio);
} else {
ratio = arc.
getCurveA() & 0x1f
;
}
trace
(" distance ratio: %d, calc=%d direct=%x,path=%x", ratio, calcRatio, direct, path
);
if (Math.
abs(ratio - calcRatio
) > 1 && (ratio
< lo || ratio
> hi
)) {
error
("Node:%x/%d: distance ratio %x, expected %x", arc.
getNode().
getOffset(),
arc.
getIndex(),
ratio, calcRatio
);
}
}
}
/**
* Check the reverse arc.
* @param arc The forward arc, from the node under test.
* @param other The target node that this arc leads towards.
*/
private void checkReverse
(RouteArc arc, RouteNode other
) {
// Find the arc from the other node that points back to us.
// There is always a reverse arc unless the arc is really a link to a
// higher road class.
boolean found =
false;
for (RouteArc rarc : other.
getArcs()) {
if (arc.
isBad())
continue;
int rlink = rarc.
getLink();
RouteNode rother = nod.
getNode(rlink
);
if (rother
!=
null && arc.
getNode() == rother
&& arc.
getNetOffset().
equals(rarc.
getNetOffset())) {
// This is the arc that links back to this node - the 'reverse arc'
compareToReverseArc
(arc, rarc
);
found =
true;
break;
}
}
if (!found
&& arc.
newNet()) {
error
("node:%x/%d: expected to find reverse arc",
arc.
getNode().
getOffset(), arc.
getIndex());
}
}
private void compareToReverseArc
(RouteArc arc, RouteArc rarc
) {
// It is believed that the signs should be different
if (!(arc.
isSign() ^ rarc.
isSign()))
error
("Node:%x/%d: reverse arc has same sign", arc.
getNode().
getOffset(), arc.
getIndex());
// Distance should be similar. Perhaps not if there are two ways of getting between nodes - need to look into this.
int d1 = arc.
getDistance();
int d2 = rarc.
getDistance();
int diff =
Math.
abs(d1 - d2
);
if (diff
> 0)
error
("Node:%x/%d: distance along reverse arc different %d/%d", arc.
getNode().
getOffset(), arc.
getIndex(), d1, d2
);
}
private void compareActual
(RouteArc arc, RouteNode node, RouteNode other
) {
Coord c1 = node.
getCoord();
Coord c2 = other.
getCoord();
compareActualDirection
(arc, c1, c2
);
compareActualDistance
(arc, node, other
);
}
private void compareActualDirection
(RouteArc arc, Coord c1, Coord c2
) {
double actualStraighLine = c1.
bearingTo(c2
);
Double direction = arc.
getInitialDegrees();
Double actualInitial = arc.
getActualInitialBearing();
trace
(" direction %.0f (%x), actual: initial=%.0f(%02x), direct=%.0f(%02x)",
direction, arc.
getDirection(), actualInitial, actualInitial
!=
null ? directionFromDegrees
( actualInitial
) :
0,
actualStraighLine, directionFromDegrees
(actualStraighLine
));
if (actualInitial ==
null)
actualInitial = actualStraighLine
;
int intInitial = directionFromDegrees
(actualInitial
);
if (compareAngle
(arc.
getDirection(), intInitial
))
error
("Node:%x/%d: direction %.0f(%02x), actual init=%.0f(%02x), direct=%.0f(%02x)",
arc.
getNode().
getOffset(), arc.
getIndex(),
direction, arc.
getDirection(),
actualInitial, intInitial,
actualStraighLine, directionFromDegrees
(actualStraighLine
));
}
private boolean compareAngle
(int a1,
int a2
) {
int allowed =
8; // Allowed difference before we complain
if ((a1
& 0x0f
) ==
0)
allowed += 0x10
;
int diff =
Math.
abs(a1 - a2
);
if (diff
> 128)
diff -=
256;
//System.out.printf("comp %d %d allowed=%x / diff=%x\n", a1, a2, allowed, diff);
return diff
> allowed
;
}
/**
* Compares the distance along the road with the value stored on the arc.
*
* @param arc The starting arc.
* @param node The starting node.
* @param other The target node that this arc leads towards.
*/
private void compareActualDistance
(RouteArc arc, RouteNode node, RouteNode other
) {
int units = arc.
getDistance();
int meters = nod.
rawToMeters(units
);
RoadData road = arc.
getRoad();
NodeLocator loc1 = road.
locateNode(node
);
NodeLocator loc2 = road.
locateNode(other
);
if (loc2 ==
null) {
error
("Node:%x/%d: Could not locate target node for arc", node.
getOffset(), arc.
getIndex());
return;
}
int pathDistance = loc2.
getLength() - loc1.
getLength();
if (!arc.
isSign())
pathDistance = -pathDistance
;
int pathUnits = nod.
metersToRaw(pathDistance
);
trace
(" distance %dm[%x/%x], actual %dm[%x] %s", meters, arc.
getDistance(), arc.
getPartialDistance(),
pathDistance, pathUnits,
(units
< pathUnits -
1)? "?short":
"");
// Check distance to within a percent.
if (Math.
abs(units - pathUnits
) > units/
100+
2)
error
("Node:%x/%d: distance %dm[%x], actual %dm[%x]", arc.
getNode().
getOffset(), arc.
getIndex(),
meters, units, pathDistance, pathUnits
);
}
public void extraArgs
(Map<String,
String> args
) {
String val = args.
remove("tab-zero");
if (val
!=
null) {
boolean z =
true;
if (val.
equals("0"))
z =
false;
RouteCenter.
setHasExtraZeros(z
);
}
super.
extraArgs(args
);
}
public static void main
(String[] args
) {
runMain
(NodCheck.
class,
"NOD", args
);
}
}