def test_vector():
try:
debug('Testing default constructor')
k = geoalgo.Vector(5)
if not k.size() == 5: raise Exception
debug('Testing copy ctor')
j = geoalgo.Vector(k)
for x in xrange(k.size()):
if not k[x] == j[x]: raise Exception
for x in xrange(5):
k[x] = 1
j[x] = 1
debug('Testing multiplication')
k *= 2
for x in xrange(5):
if not k[x] == 2: raise Exception
debug('Testing addition')
k += j
for x in xrange(5):
if not k[x] == 3: raise Exception
debug('Testing division')
k /= 3.
for x in xrange(5):
if not k[x] == 1.: raise Exception
debug('Testing dot product')
if not k * j == 5: raise Exception
debug('Testing length')
for x in xrange(5):
k[x] = 0
k[0] = 1
if not k.Length() == 1: raise Exception
debug('Testing compatibility check')
error = False
try:
k.compat(j)
except Exception:
pass
if error: raise Exception
except Exception:
error('geoalgo::Vector unit test failed.')
print(traceback.format_exception(*sys.exc_info())[2])
return 1
info('geoalgo::Vector unit test complete.')
return 0
def test_dAlgo():
debug()
debug(BLUE +
"Precision Being Required to Consider Two numbers Equal: {0:.2e}".
format(_epsilon) + ENDC)
debug()
# number of times to test each function
tests = 10000
# import Distance Algo
dAlgo = geoalgo.GeoAlgo()
try:
# test distance from point to infinite line
info('Testing Point & Infinite Line Distance')
totSuccess = 0
sqdistT = 0
closestT = 0
for y in range(tests):
success = 1
# generate a random point (will be point on line closest to external point
p1 = geoalgo.Vector(random(), random(), random())
# second point on line
p2 = geoalgo.Vector(random(), random(), random())
# generate random line
l = geoalgo.Line(p1, p2)
# generate a point in a direction transverse to the line
transX = random()
transY = random()
# now ensure perpendicularity by having dot product be == 0
dirct = l.Pt2() - l.Pt1()
transZ = (-transX * dirct[0] - transY * dirct[1]) / dirct[2]
vectTranslate = geoalgo.Vector(transX, transY, transZ)
# generate point away starting from p1
pt = geoalgo.Vector(p1 + vectTranslate)
# answer will be vectTranslate sq. length
answer = vectTranslate.SqLength()
# closest point will be p1
tim = time()
a1 = dAlgo.SqDist(pt, l)
sqdistT += (time() - tim)
a2 = dAlgo.SqDist(l, pt)
tim = time()
pAnswer1 = dAlgo.ClosestPt(pt, l)
closestT += (time() - tim)
pAnswer2 = dAlgo.ClosestPt(l, pt)
if not (abs(answer - a1) < _epsilon): success = 0
if not (abs(answer - a2) < _epsilon): success = 0
for x in xrange(3):
if not (abs(p1[x] - pAnswer1[x]) < _epsilon): success = 0
if not (abs(p1[x] - pAnswer2[x]) < _epsilon): success = 0
totSuccess += success
if (float(totSuccess) / tests < 1):
info(NO + "Success: {0}%".format(100 * float(totSuccess) / tests) +
ENDC)
else:
info(OK + "Success: {0}%".format(100 * float(totSuccess) / tests) +
ENDC)
info("Time for SqDist : {0:.3f} us".format(
1E6 * sqdistT / tests))
info("Time for ClosestPt : {0:.3f} us".format(
1E6 * closestT / tests))
# test distance from point to segment
info('Testing Point & LineSegment Distance')
totSuccess = 0
sqdistT_out = 0.
closestT_out = 0.
sqdistT_in = 0.
closestT_in = 0.
for y in range(tests):
success = 1
# generate a random segment
l = geoalgo.LineSegment(random(), random(), random(), random(),
random(), random())
# get the segment length
lLen = l.Dir().Length()
# get the segment direction
d = l.Dir()
# generate a segment parallel to it
# to get a line parallel to the first,
# translate the Start and End points
# by some amount in a direction perpendicular
# to that of the original line
transX = random()
transY = random()
# now ensure perpendicularity by having dot product be == 0
transZ = (-transX * d[0] - transY * d[1]) / d[2]
vectTranslate = geoalgo.Vector(transX, transY, transZ)
p1 = l.Start() + vectTranslate
p2 = l.End() + vectTranslate
# parallel segment:
lPar = geoalgo.LineSegment(p1, p2)
# first, test a point that is "before start point"
# distance to this point should be distance between lines
# in quadrature with how much further from start point we go
dist = random()
# direction vector outwards from line
dirOut = lPar.Dir() * (-1 * dist / lPar.Dir().Length())
pTest = p1 + dirOut
answer = dirOut.SqLength() + vectTranslate.SqLength()
tim = time()
a1 = dAlgo.SqDist(pTest, l)
sqdistT_out += (time() - tim)
a2 = dAlgo.SqDist(l, pTest)
if not (abs(answer - a1) < _epsilon): success = 0
if not (abs(answer - a2) < _epsilon): success = 0
tim = time()
point1 = dAlgo.ClosestPt(pTest, l)
closestT_out += (time() - tim)
point2 = dAlgo.ClosestPt(l, pTest)
for x in xrange(3):
if not ((l.Start()[x] - point1[x]) < _epsilon): success = 0
if not ((l.Start()[x] - point2[x]) < _epsilon): success = 0
# now, test a point inside the segment.
# distance between point & segment should be
# pre-determined distance between the two segments
dist = random()
dirIn = lPar.Dir() * dist #dist ensures < distance of full segment
pTest = p1 + dirIn
answer = vectTranslate.SqLength()
tim = time()
a1 = dAlgo.SqDist(pTest, l)
sqdistT_in += (time() - tim)
a2 = dAlgo.SqDist(l, pTest)
if not (abs(answer - a1) < _epsilon): success = 0
if not (abs(answer - a2) < _epsilon): success = 0
pAns = l.Start() + dirIn
tim = time()
point1 = dAlgo.ClosestPt(pTest, l)
closestT_in += (time() - tim)
point2 = dAlgo.ClosestPt(l, pTest)
for x in xrange(3):
if not ((pAns[x] - point1[x]) < _epsilon): success = 0
if not ((pAns[x] - point2[x]) < _epsilon): success = 0
# now test a point beyond the segment
dist = random()
dirOut = lPar.Dir() * (dist / lPar.Dir().Length())
pTest = p2 + dirOut
answer = dirOut.SqLength() + vectTranslate.SqLength()
if not (abs(answer - dAlgo.SqDist(pTest, l)) < _epsilon):
success = 0
if not (abs(answer - dAlgo.SqDist(l, pTest)) < _epsilon):
success = 0
point1 = dAlgo.ClosestPt(pTest, l)
point2 = dAlgo.ClosestPt(pTest, l)
for x in xrange(3):
if not ((l.End()[x] - point1[x]) < _epsilon): success = 0
if not ((l.End()[x] - point2[x]) < _epsilon): success = 0
if (success == 1): totSuccess += 1
if (float(totSuccess) / tests < 1):
info(NO + "Success: {0}%".format(100 * float(totSuccess) / tests) +
ENDC)
else:
info(OK + "Success: {0}%".format(100 * float(totSuccess) / tests) +
ENDC)
info("Time for SqDist (Pt Out of Segment) : {0:.3f} us".format(
1E6 * sqdistT_out / tests))
info("Time for ClosestPt (Pt Out of Segment): {0:.3f} us".format(
1E6 * closestT_out / tests))
info("Time for SqDist (Pt In Segment) : {0:.3f} us".format(
1E6 * sqdistT_in / tests))
info("Time for ClosestPt (Pt In Segment) : {0:.3f} us".format(
1E6 * closestT_in / tests))
# test Point to HalfLine distance
debug('Testing Point & HalfLine Distance')
success = 1
totSuccess = 0
sqdistT_out = 0.
closestT_out = 0.
sqdistT_in = 0.
closestT_in = 0.
for x in range(tests):
# generate a random segment
l = geoalgo.HalfLine(random(), random(), random(), random(),
random(), random())
# generate a segment parallel to it
# to get a line parallel to the first,
# translate the Start and End points
# by some amount in a direction perpendicular
# to that of the original line
transX = random()
transY = random()
# now ensure perpendicularity by having dot product be == 0
transZ = (-transX * l.Dir()[0] - transY * l.Dir()[1]) / l.Dir()[2]
vectTranslate = geoalgo.Vector(transX, transY, transZ)
# create the new translated & parallel hal-fline
lPar = geoalgo.HalfLine(l.Start() + vectTranslate, l.Dir())
# first, test a point that is "before start point"
# distance to this point should be distance between lines
# in quadrature with how much further from start point we go
dist = random()
# direction vector outwards from line
dirOut = lPar.Dir() * (-1 * dist)
pTest = lPar.Start() + dirOut
answer = dirOut.SqLength() + vectTranslate.SqLength()
tim = time()
a1 = dAlgo.SqDist(pTest, l)
tim = time() - tim
sqdistT_out += tim
a2 = dAlgo.SqDist(l, pTest)
if not (abs(answer - a1) < _epsilon): success = 0
if not (abs(answer - a2) < _epsilon): success = 0
tim = time()
point1 = dAlgo.ClosestPt(pTest, l)
tim = time() - tim
closestT_out += tim
point2 = dAlgo.ClosestPt(l, pTest)
for x in xrange(3):
if not ((l.Start()[x] - point1[x]) < _epsilon): success = 0
if not ((l.Start()[x] - point2[x]) < _epsilon): success = 0
# now, test a point inside the segment.
# distance between point & segment should be
# pre-determined distance between the two segments
dist = random()
dirIn = lPar.Dir() * dist #dist ensures < distance of full segment
pTest = lPar.Start() + dirIn
answer = vectTranslate.SqLength()
pAns = l.Start() + dirIn
tim = time()
a1 = dAlgo.SqDist(pTest, l)
tim = time() - tim
sqdistT_in += tim
a2 = dAlgo.SqDist(l, pTest)
if not (abs(answer - a1) < _epsilon): success = 0
if not (abs(answer - a2) < _epsilon): success = 0
tim = time()
point1 = dAlgo.ClosestPt(pTest, l)
tim = time() - tim
closestT_in += tim
point2 = dAlgo.ClosestPt(l, pTest)
for x in xrange(3):
if not ((pAns[x] - point1[x]) < _epsilon): success = 0
if not ((pAns[x] - point2[x]) < _epsilon): success = 0
if (success == 1): totSuccess += 1
if (float(totSuccess) / tests < 1):
info(NO + "Success: {0}%".format(100 * float(totSuccess) / tests) +
ENDC)
else:
info(OK + "Success: {0}%".format(100 * float(totSuccess) / tests) +
ENDC)
info("Time for SqDist (Pt Out of Segment) : {0:.3f} us".format(
1E6 * sqdistT_out / tests))
info("Time for ClosestPt (Pt Out of Segment): {0:.3f} us".format(
1E6 * closestT_out / tests))
info("Time for SqDist (Pt In Segment) : {0:.3f} us".format(
1E6 * sqdistT_in / tests))
info("Time for ClosestPt (Pt In Segment) : {0:.3f} us".format(
1E6 * closestT_in / tests))
# test Distance between two Infinite Lines
debug('Testing Inf Line & Inf Line Distance')
totSuccess = 0
sqdistT = 0.
closestT = 0.
for y in range(tests):
success = 1
l1 = geoalgo.Line(random(), random(), random(), random(), random(),
random())
# take a point a fixed distance away
# generate a random direction in the plane
# perpendicular to the plane connecting
# the point and the line
# the distance between the two lines
# should be the fixed amount selected previously
# use half-way point to do calculations
p1 = (l1.Pt2() + l1.Pt1()) / 2
# get line direction
d1 = (l1.Pt2() - l1.Pt1())
# move in a random direction perpendicular to line
dirx = random()
diry = random()
dirz = (-dirx * d1[0] - diry * d1[1]) / d1[2]
vectTranslate = geoalgo.Vector(dirx, diry, dirz)
# need to re-orient in some random direction on this plane
# this direction has to be perpendicular to both
# the line's direction as well as to the direction
# of the segment uniting the two lines
# use cross-product
vectRotate = vectTranslate.Cross(d1)
l2 = geoalgo.Line(p1 + vectTranslate,
p1 + vectTranslate + vectRotate)
# now calculate distance. Should be vectTranslate.Length()
answer = vectTranslate.SqLength()
tim = time()
a1 = dAlgo.SqDist(l1, l2)
tim = time() - tim
sqdistT += tim
# expect the closest points on both lines to be p1 & l2.Pt1()
ptL1 = geoalgo.Vector()
ptL2 = geoalgo.Vector()
a2 = dAlgo.SqDist(l1, l2, ptL1, ptL2)
if not (abs(answer - a1) < _epsilon): success = 0
if not (abs(answer - a2) < _epsilon): success = 0
for x in xrange(3):
if not (abs(ptL1[x] - p1[x]) < _epsilon): success = 0
if not (abs(ptL2[x] - l2.Pt1()[x]) < _epsilon): success = 0
if (success == 1): totSuccess += 1
if (float(totSuccess) / tests < 1):
info(NO + "Success: {0}%".format(100 * float(totSuccess) / tests) +
ENDC)
else:
info(OK + "Success: {0}%".format(100 * float(totSuccess) / tests) +
ENDC)
info("Time for SqDist : {0:.3f} us".format(
1E6 * sqdistT / tests))
# test Distance between two Half-Infinite Lines
debug('Testing Half-Inf Line & Half-Inf Line Distance')
totSuccess = 0
sqdistT_in = 0
timesIN = 0
sqdistT_out = 0
timesOUT = 0
for y in range(tests):
success = 1
l1 = geoalgo.HalfLine(random(), random(), random(), random(),
random(), random())
# take a point a fixed distance away
# then generate a new half-line starting
# at the same point (translated) and with
# the same (or opposite) direction.
# But rotated outward a bit
p1 = l1.Start()
# get line direction
d1 = l1.Dir()
# move in a random direction perpendicular to line
dirx = random()
diry = random()
dirz = (-dirx * d1[0] - diry * d1[1]) / d1[2]
vectTranslate = geoalgo.Vector(dirx, diry, dirz)
# pick some random direction (aligned with 1st or not)
aligned = -1
if (random() < 0.5): aligned = 1
l2 = geoalgo.HalfLine(p1 + vectTranslate,
d1 + vectTranslate * aligned)
# now calculate distance.
# if aligned == -1 distance should be == 0 (lines intersect)
# if aligned == 1 then the two start points are the closest points
if (aligned == -1):
timesIN += 1
answer = 0.
tim = time()
a1 = dAlgo.SqDist(l1, l2)
tim = time() - tim
L1 = geoalgo.Vector()
L2 = geoalgo.Vector()
a2 = dAlgo.SqDist(l1, l2, L1, L2)
sqdistT_in += tim
if not (abs(answer - a1) < _epsilon): success = 0
if not (abs(answer - a2) < _epsilon): success = 0
for x in xrange(3):
if not (L1[x] - L2[x] < _epsilon): success = 0
if (aligned == 1):
timesOUT += 1
answer = vectTranslate.SqLength()
tim = time()
a1 = dAlgo.SqDist(l1, l2)
tim = time() - tim
L1 = geoalgo.Vector()
L2 = geoalgo.Vector()
a2 = dAlgo.SqDist(l1, l2, L1, L2)
sqdistT_out += tim
if not (abs(answer - a1) < _epsilon): success = 0
if not (abs(answer - a2) < _epsilon): success = 0
for x in xrange(3):
if not (L1[x] - l1.Start()[x] < _epsilon): success = 0
if not (L2[x] - l2.Start()[x] < _epsilon): success = 0
if (success == 1): totSuccess += 1
if (float(totSuccess) / tests < 1):
info(NO + "Success: {0}%".format(100 * float(totSuccess) / tests) +
ENDC)
else:
info(OK + "Success: {0}%".format(100 * float(totSuccess) / tests) +
ENDC)
info("Time for SqDist (OUT) : {0:.3f} us".format(
1E6 * sqdistT_out / timesOUT))
info("Time for SqDist (IN) : {0:.3f} us".format(
1E6 * sqdistT_in / timesIN))
# test Distance between Half-Line and Line Segment
debug('Testing Half Line & Line Segment Distance')
totSuccess1 = 0
sqdistT1 = 0
totSuccess2 = 0
sqdistT2 = 0
totSuccess3 = 0
sqdistT3 = 0
totSuccess4 = 0
sqdistT4 = 0
for y in range(tests):
success1 = 1
success2 = 1
success3 = 1
success4 = 1
l1 = geoalgo.HalfLine(random(), random(), random(), random(),
random(), random())
# take a point a fixed distance away
# test multiple scenarios:
# 1) segment "away" from half-line but same direction
# -> closest points are half-line start and segment end
# 2) segment "away" from half-line and rotated
# -> closest points are half-line start and segment rotation pivot
# 3) segment "close to" half-line and tilted outwards
# -> closest points are point on half-line and segment start
# 4) segment "close to" half-line and rotated
# -> closest points are point on half-line and segment rotation pivot
p1 = l1.Start()
# get line direction
d1 = l1.Dir()
# move in a random direction perpendicular to line
dirx = random()
diry = random()
dirz = (-dirx * d1[0] - diry * d1[1]) / d1[2]
vectTranslate = geoalgo.Vector(dirx, diry, dirz)
# 1) generate line-segment "behind" half-line
# use unit-vector nature of Dir() to go back slightly more
dist = random()
seg = geoalgo.LineSegment(p1 + vectTranslate - d1 * dist,
p1 + vectTranslate - d1 * dist - d1)
# distance should be dist
# closest points should be seg.Start() and l1.Start()
answer = vectTranslate * vectTranslate + dist * dist
tim = time()
a1 = dAlgo.SqDist(l1, seg)
tim = time() - tim
sqdistT1 += tim
L1 = geoalgo.Vector()
L2 = geoalgo.Vector()
a2 = dAlgo.SqDist(l1, seg, L1, L2)
if not (abs(answer - a1) < _epsilon): success1 = 0
if not (abs(answer - a2) < _epsilon): success1 = 0
for x in xrange(3):
if not (L1[x] - l1.Start()[x] < _epsilon): success1 = 0
if not (L2[x] - seg.Start()[x] < _epsilon): success1 = 0
if (success1 == 1): totSuccess1 += 1
# 2) generate line segment away but rotated
# rotate in a direction perpendicular to both line direction and translation direction
vectRotate = vectTranslate.Cross(d1)
# choose pivot point
pivot = p1 + vectTranslate - d1 * dist
seg = geoalgo.LineSegment(pivot - vectRotate * random(),
pivot + vectRotate * random())
# distance should be pivot distance to half-line start
# closest points should be pivot and line start point
answer = vectTranslate * vectTranslate + dist * dist
tim = time()
a1 = dAlgo.SqDist(l1, seg, L1, L2)
sqdistT2 += (time() - tim)
a2 = dAlgo.SqDist(seg, l1)
if not (abs(answer - a1) < _epsilon): success2 = 0
if not (abs(answer - a2) < _epsilon): success2 = 0
for x in xrange(3):
if not (L1[x] - l1.Start()[x] < _epsilon): success2 = 0
if not (L2[x] - pivot[x] < _epsilon): success2 = 0
if (success2 == 1): totSuccess2 += 1
# 3) generate line segment next to but tilted outwards
seg = geoalgo.LineSegment(
p1 + vectTranslate + d1 * dist,
p1 + vectTranslate + d1 * dist + vectTranslate * random())
# distance should be vectTranslate
# closest point should be segment start and point on line "d1*dist" away from line-start
answer = vectTranslate * vectTranslate
tim = time()
a1 = dAlgo.SqDist(l1, seg, L1, L2)
sqdistT3 += (time() - tim)
a2 = dAlgo.SqDist(seg, l1)
if not (abs(answer - a1) < _epsilon): success3 = 0
if not (abs(answer - a2) < _epsilon): success3 = 0
ptLine = l1.Start() + d1 * dist
for x in xrange(3):
if not (L1[x] - ptLine[x] < _epsilon): success3 = 0
if not (L2[x] - seg.Start()[x] < _epsilon): success3 = 0
if (success3 == 1): totSuccess3 += 1
# 4) generate line segment next to line but rotated
pivot = p1 + vectTranslate + d1 * dist
seg = geoalgo.LineSegment(pivot - vectRotate * random(),
pivot + vectRotate * random())
# distance should be vectTranslate
# closest point should be pivot on segment and d1*dist away from start for line
answer = vectTranslate * vectTranslate
tim = time()
a1 = dAlgo.SqDist(l1, seg, L1, L2)
sqdistT4 += (time() - tim)
a2 = dAlgo.SqDist(seg, l1)
if not (abs(answer - a1) < _epsilon): success4 = 0
if not (abs(answer - a2) < _epsilon): success4 = 0
ptLine = l1.Start() + d1 * dist
for x in xrange(3):
if not (L1[x] - ptLine[x] < _epsilon): success4 = 0
if not (L2[x] - pivot[x] < _epsilon): success4 = 0
if (success4 == 1): totSuccess4 += 1
if (float(totSuccess1) / tests < 1):
info(NO +
"Success: {0}%".format(100 * float(totSuccess1) / tests) +
ENDC)
else:
info(OK +
"Success: {0}%".format(100 * float(totSuccess1) / tests) +
ENDC)
info("Time for SqDist (Case 1) : {0:.3f} us".format(
1E6 * sqdistT1 / tests))
if (float(totSuccess2) / tests < 1):
info(NO +
"Success: {0}%".format(100 * float(totSuccess2) / tests) +
ENDC)
else:
info(OK +
"Success: {0}%".format(100 * float(totSuccess2) / tests) +
ENDC)
info("Time for SqDist (Case 2) : {0:.3f} us".format(
1E6 * sqdistT2 / tests))
if (float(totSuccess3) / tests < 1):
info(NO +
"Success: {0}%".format(100 * float(totSuccess3) / tests) +
ENDC)
else:
info(OK +
"Success: {0}%".format(100 * float(totSuccess3) / tests) +
ENDC)
info("Time for SqDist (Case 3) : {0:.3f} us".format(
1E6 * sqdistT3 / tests))
if (float(totSuccess4) / tests < 1):
info(NO +
"Success: {0}%".format(100 * float(totSuccess4) / tests) +
ENDC)
else:
info(OK +
"Success: {0}%".format(100 * float(totSuccess4) / tests) +
ENDC)
info("Time for SqDist (Case 4) : {0:.3f} us".format(
1E6 * sqdistT4 / tests))
# test Distance between Half-Line and Line Segment
debug('Testing Line Segment & Line Segment Distance')
totSuccess1 = 0.
sqdistT1 = 0
totSuccess2 = 0.
sqdistT2 = 0
totSuccess3 = 0.
sqdistT3 = 0
totSuccess4 = 0.
sqdistT4 = 0
for y in range(tests):
success1 = 1
success2 = 1
success3 = 1
success4 = 1
seg1 = geoalgo.LineSegment(random(), random(), random(), random(),
random(), random())
# take a point a fixed distance away
# test multiple scenarios:
# 1) segment "away" from half-line but same direction
# -> closest points are half-line start and segment end
# 2) segment "away" from half-line and rotated
# -> closest points are half-line start and segment rotation pivot
# 3) segment "close to" half-line and tilted outwards
# -> closest points are point on half-line and segment start
# 4) segment "close to" half-line and rotated
# -> closest points are point on half-line and segment rotation pivot
p1 = seg1.Start()
# get line direction
p2 = seg1.End()
d = p2 - p1
length = d.Length()
# move in a random direction perpendicular to line
dirx = random()
diry = random()
dirz = (-dirx * d[0] - diry * d[1]) / d[2]
vectTranslate = geoalgo.Vector(dirx, diry, dirz)
# 1) generate line-segment "behind" half-line
# use unit-vector nature of Dir() to go back slightly more
dist = random()
seg = geoalgo.LineSegment(p1 + vectTranslate - d * dist,
p1 + vectTranslate - d * dist - d)
#seg = geoalgo.LineSegment(p1+vectTranslate,p1+vectTranslate-d)
# distance should be dist
# closest points should be seg.Start() and seg1.Start()
answer = vectTranslate * vectTranslate + dist * dist * d.SqLength()
tim = time()
a1 = dAlgo.SqDist(seg1, seg)
tim = time() - tim
sqdistT1 += tim
L1 = geoalgo.Vector()
L2 = geoalgo.Vector()
a2 = dAlgo.SqDist(seg1, seg, L1, L2)
if not (abs(answer - a1) < _epsilon): success1 = 0
if not (abs(answer - a2) < _epsilon): success1 = 0
for x in xrange(3):
if not (L1[x] - seg1.Start()[x] < _epsilon): success1 = 0
if not (L2[x] - seg.Start()[x] < _epsilon): success1 = 0
if (success1 == 1): totSuccess1 += 1
# 2) generate line segment away but rotated
# rotate in a direction perpendicular to both line direction and translation direction
vectRotate = vectTranslate.Cross(d)
# choose pivot point
pivot = p1 + vectTranslate - d * dist
seg = geoalgo.LineSegment(pivot - vectRotate * random(),
pivot + vectRotate * random())
# distance should be pivot distance to half-line start
# closest points should be pivot and line start point
answer = vectTranslate * vectTranslate + dist * dist * d.SqLength()
tim = time()
a1 = dAlgo.SqDist(seg1, seg, L1, L2)
sqdistT2 += (time() - tim)
a2 = dAlgo.SqDist(seg, seg1)
if not (abs(answer - a1) < _epsilon): success2 = 0
if not (abs(answer - a2) < _epsilon): success2 = 0
for x in xrange(3):
if not (L1[x] - seg1.Start()[x] < _epsilon): success2 = 0
if not (L2[x] - pivot[x] < _epsilon): success2 = 0
if (success2 == 1): totSuccess2 += 1
# 3) generate line segment next to but tilted outwards
distin = length * random(
) # ensures that we do not pass the segment
seg = geoalgo.LineSegment(
p1 + vectTranslate + d * distin,
p1 + vectTranslate + d * distin + vectTranslate * random())
# distance should be vectTranslate
# closest point should be segment start and point on line "d*distin" away from line-start
answer = vectTranslate * vectTranslate
tim = time()
a1 = dAlgo.SqDist(seg1, seg, L1, L2)
sqdistT3 += (time() - tim)
a2 = dAlgo.SqDist(seg, seg1)
if not (abs(answer - a1) < _epsilon): success3 = 0
if not (abs(answer - a2) < _epsilon): success3 = 0
ptLine = seg1.Start() + d * distin
for x in xrange(3):
if not (L1[x] - ptLine[x] < _epsilon): success3 = 0
if not (L2[x] - seg.Start()[x] < _epsilon): success3 = 0
if (success3 == 1): totSuccess3 += 1
# 4) generate line segment next to line but rotated
pivot = p1 + vectTranslate + d * distin
seg = geoalgo.LineSegment(pivot - vectRotate * random(),
pivot + vectRotate * random())
# distance should be vectTranslate
# closest point should be pivot on segment and d*distin away from start for line
answer = vectTranslate * vectTranslate
tim = time()
a1 = dAlgo.SqDist(seg1, seg, L1, L2)
sqdistT4 += (time() - tim)
a2 = dAlgo.SqDist(seg, seg1)
if not (abs(answer - a1) < _epsilon): success4 = 0
if not (abs(answer - a2) < _epsilon): success4 = 0
ptLine = seg1.Start() + d * distin
for x in xrange(3):
if not (L1[x] - ptLine[x] < _epsilon): success4 = 0
if not (L2[x] - pivot[x] < _epsilon): success4 = 0
if (success4 == 1): totSuccess4 += 1
if (float(totSuccess1) / tests < 1):
info(NO +
"Success: {0}%".format(100 * float(totSuccess1) / tests) +
ENDC)
else:
info(OK +
"Success: {0}%".format(100 * float(totSuccess1) / tests) +
ENDC)
info("Time for SqDist (Case 1) : {0:.3f} us".format(
1E6 * sqdistT1 / tests))
if (float(totSuccess2) / tests < 1):
info(NO +
"Success: {0}%".format(100 * float(totSuccess2) / tests) +
ENDC)
else:
info(OK +
"Success: {0}%".format(100 * float(totSuccess2) / tests) +
ENDC)
info("Time for SqDist (Case 2) : {0:.3f} us".format(
1E6 * sqdistT2 / tests))
if (float(totSuccess3) / tests < 1):
info(NO +
"Success: {0}%".format(100 * float(totSuccess3) / tests) +
ENDC)
else:
info(OK +
"Success: {0}%".format(100 * float(totSuccess3) / tests) +
ENDC)
info("Time for SqDist (Case 3) : {0:.3f} us".format(
1E6 * sqdistT3 / tests))
if (float(totSuccess4) / tests < 1):
info(NO + "Success: {0}%".format(100 * float(totSuccess4) /
float(tests)) + ENDC)
else:
info(OK + "Success: {0}%".format(100 * float(totSuccess4) /
float(tests)) + ENDC)
info("Time for SqDist (Case 4) : {0:.3f} us".format(
1E6 * sqdistT4 / tests))
# test Distance between Point and AABox
debug('Testing Point and AABox Distance/Closest Point')
success1 = 1
totSuccess1 = 0.
sqdistT1 = 0
closestT1 = 0
success2 = 1
totSuccess2 = 0.
sqdistT2 = 0
closestT2 = 0
success3 = 1
totSuccess3 = 0.
sqdistT3 = 0
closestT3 = 0
success4 = 1
totSuccess4 = 0.
sqdistT4 = 0
closestT4 = 0
for y in xrange(tests):
success1 = 1
success2 = 1
success3 = 1
success4 = 1
# create a simple cubic box from 0,0,0 to 1,1,1
b = geoalgo.AABox(0, 0, 0, 1, 1, 1)
# various cases to test:
# case 1) Point inside box
# case 2) Point outside box
# 1) POINT INSIDE BOX
p = geoalgo.Vector(random(), random(), random())
# if point not recognized as inside -> error!
if not (b.Contain(p)): success1 = 0
dTop = 1 - p[2]
dBot = p[2]
dLeft = p[0]
dRight = 1 - p[0]
dFront = p[1]
dBack = 1 - p[1]
# find smallest of these
dMin = dTop
if (dBot < dMin): dMin = dBot
if (dLeft < dMin): dMin = dLeft
if (dRight < dMin): dMin = dRight
if (dFront < dMin): dMin = dFront
if (dBack < dMin): dMin = dBack
answer = dMin * dMin
tim = time()
a1 = dAlgo.SqDist(p, b)
sqdistT1 += (time() - tim)
a2 = dAlgo.SqDist(b, p)
# closest point
tim = time()
pt1 = dAlgo.ClosestPt(b, p)
closestT1 += (time() - tim)
pt2 = dAlgo.ClosestPt(p, b)
if not (abs(answer - a1) < _epsilon): success1 = 0
if not (abs(answer - a2) < _epsilon): success1 = 0
for x in xrange(3):
if not (pt1[x] - p[x] < _epsilon): success1 = 0
if not (pt2[x] - p[x] < _epsilon): success1 = 0
if (success1 == 1): totSuccess1 += 1
# 2) POINT OUT OF BOX
# pick a side that is should exit on at random
pick = random()
side = 0
if ((pick > 0.33) and (pick < 0.67)): side = 1
if (pick > 0.67): side = 2
# pick a direction to overflow the box by (-1 = back or +1 = front)
direction = 1
if (random() < 0.5): direction = -1
if (side == 0):
p = geoalgo.Vector(random() + direction, random(), random())
elif (side == 1):
p = geoalgo.Vector(random(), random() + direction, random())
else:
p = geoalgo.Vector(random(), random(), random() + direction)
# if point not recognized as outside -> error!
if (b.Contain(p)): success1 = 0
# go through cases and find min distance
if ((side == 0) and (direction == 1)):
#overflow on +x direction
dMin = p[0] - 1
pMin = geoalgo.Vector(1, p[1], p[2])
if ((side == 0) and (direction == -1)):
#overflow on -x direction
dMin = -p[0]
pMin = geoalgo.Vector(0, p[1], p[2])
if ((side == 1) and (direction == 1)):
#overflow on +y direction
dMin = p[1] - 1
pMin = geoalgo.Vector(p[0], 1, p[2])
if ((side == 1) and (direction == -1)):
#overflow on -y direction
dMin = -p[1]
pMin = geoalgo.Vector(p[0], 0, p[2])
if ((side == 2) and (direction == 1)):
#overflow on +z direction
dMin = p[2] - 1
pMin = geoalgo.Vector(p[0], p[1], 1)
if ((side == 2) and (direction == -1)):
#overflow on -z direction
dMin = -p[2]
pMin = geoalgo.Vector(p[0], p[1], 0)
answer = dMin * dMin
tim = time()
a1 = dAlgo.SqDist(p, b)
sqdistT2 += (time() - tim)
a2 = dAlgo.SqDist(b, p)
# closest point
tim = time()
pt1 = dAlgo.ClosestPt(b, p)
closestT2 += (time() - tim)
pt2 = dAlgo.ClosestPt(p, b)
#info("Point: [{0}, {1}, {2}]".format(p[0],p[1],p[2]))
#info("Expected: {0}. Got: {1}".format(answer,a1))
#info("Expected: {0}. Got: {1}".format(answer,a2))
if not (abs(answer - a1) < _epsilon): success2 = 0
if not (abs(answer - a2) < _epsilon): success2 = 0
for x in xrange(3):
#info("\tExpected: {0}. Got: {1}".format(p[x],pt1[x]))
#info("\tExpected: {0}. Got: {1}".format(p[x],pt2[x]))
if not (pt1[x] - pMin[x] < _epsilon): success2 = 0
if not (pt2[x] - pMin[x] < _epsilon): success2 = 0
#info("Success: {0}".format(success2))
if (success2 == 1): totSuccess2 += 1
if (float(totSuccess1) / tests < 1):
info(NO +
"Success: {0}%".format(100 * float(totSuccess1) / tests) +
ENDC)
else:
info(OK +
"Success: {0}%".format(100 * float(totSuccess1) / tests) +
ENDC)
info("Time for SqDist (Case 1) : {0:.3f} us".format(
1E6 * sqdistT1 / tests))
info("Time for ClosestPt (Case 1) : {0:.3f} us".format(
1E6 * closestT1 / tests))
if (float(totSuccess2) / tests < 1):
info(NO +
"Success: {0}%".format(100 * float(totSuccess2) / tests) +
ENDC)
else:
info(OK +
"Success: {0}%".format(100 * float(totSuccess2) / tests) +
ENDC)
info("Time for SqDist (Case 2) : {0:.3f} us".format(
1E6 * sqdistT2 / tests))
info("Time for ClosestPt (Case 2) : {0:.3f} us".format(
1E6 * closestT2 / tests))
except Exception:
error('geoalgo::DistanceAlgo unit test failed.')
print traceback.format_exception(*sys.exc_info())[2]
return 1
info('geoalgo::DistanceAlgo unit test complete.')
return 0
from basictool import GeoViewer
from ROOT import geoalgo
k = GeoViewer()
xs_alg = geoalgo.GeoAlgo()
pt = geoalgo.Vector(3)
trj = geoalgo.Trajectory()
for x in xrange(12):
for y in xrange(pt.size()):
if x < 5 or not y:
pt[y] = x - 2
elif y >= 1:
pt[y] = 5
print 'Adding trajectory point:', x, ' ... ', pt[0], pt[1], pt[2]
trj += pt
box = geoalgo.AABox(0, 0, 0, 7, 7, 7)
k.add(box, 'Test Box')
k.add(trj, 'Test Trajectory')
xs_pts = xs_alg.Intersection(trj, box)
for x in xrange(xs_pts.size()):
k.add(xs_pts[x], 'XS Point')
k.show()
from ROOT import geoalgo
from basictool.geoviewer import GeoViewer
import matplotlib.pyplot as plt
import random as rnd
import time
import datetime
plt.ion()
tottime = 0
# track:
viewer = GeoViewer()
viewer._use_box = False
geoAlgo = geoalgo.GeoAlgo()
line1 = geoalgo.LineSegment(0, 0, 0, 0, 0, 1)
line2 = geoalgo.LineSegment(1, 0, 0, 1, 0, 1)
pt1 = geoalgo.Vector()
pt2 = geoalgo.Vector()
d = geoAlgo.SqDist(line1, line2, pt1, pt2)
line3 = geoalgo.LineSegment(pt1, pt2)
viewer.add(line1, 'ln1', 'b')
viewer.add(line2, 'ln2', 'r')
viewer.add(line3, 'ln3', 'k')
print 'distance is %.04f' % d
viewer.show()
try:
counter = input('Hit ENTER when you are ready to close the viewer')
except:
print "done with this example..."
import ROOT
from ROOT import ertool
from ROOT import geoalgo
from basictool.geoviewer import GeoViewer
import matplotlib.pyplot as plt
#
# Make my fake data input
#
# Make a fake track and shower
# shower:
shrDir = geoalgo.Vector(0.3, 1.0, 0.5) # direction
shrStart = geoalgo.Vector(10, -10, 30) # start point
shower = ertool.Shower(shrStart, shrDir, 20, 5)
shower._energy = 100
shower._dedx = 2.2
# track:
track = ertool.Track()
for n in xrange(11):
track.push_back(
geoalgo.Vector(-45.1 + (10 + 45) * (n / 10.),
15.1 - (10 + 15) * (n / 10.),
4.1 + (30 - 4) * (n / 10.)))
track._energy = 435
plt.ion()
viewer = GeoViewer()
viewer.add(shower, 'shower', 'b')
viewer.add(track, 'track', 'r')
viewer.show()
def test_dAlgo():
debug()
debug(BLUE +
"Precision Being Required to Consider Two numbers Equal: {0:.2e}".
format(_epsilon) + ENDC)
debug()
# number of times to test each function
tests = 10000
# import Distance Algo
iAlgo = geoalgo.GeoAlgo()
try:
# test distance to and back from wall
info('Testing Intersection Between Half-Line & AABox')
totSuccess_f = 0
intersectT_f = 0
totSuccess_b = 0
intersectT_b = 0
for y in range(tests):
success_f = 1
success_b = 1
# generate a unit cubic box from (0,0,0) to (1,1,1)
box = geoalgo.AABox(0, 0, 0, 1, 1, 1)
# generate random point inside box
# half-line will start from this point
p = geoalgo.Vector(random(), random(), random())
# now find random intersection point
# pick a side of the box that the point is on
pick = random()
side = 0
if ((pick > 0.33) and (pick < 0.67)): side = 1
if (pick > 0.67): side = 2
# pick a direction (Left/Right), (Top/Bottom), (Front/Back) that the point should be on
# given the direction & side, find the intersection point on the relevant face of the cube. That will be our intersection point. Make the half-line pass through there
direction = 1
if (random() < 0.5): direction = -1
# pl is a parameter to know numerical value of coordinate for the face we are on
# if direction = 1 -> pl = +1 (box.Max())
# if direction = -1 -> pl = -1 (box.Min())
pl = 1
if (direction == -1):
pl = 0
if (side == 0):
i = geoalgo.Vector(pl, random(), random())
elif (side == 1):
i = geoalgo.Vector(random(), pl, random())
else:
i = geoalgo.Vector(random(), random(), pl)
# now generate half-line passing thorugh p and i & starting from p
d = i - p
lin = geoalgo.HalfLine(p, d)
# answer should be distance between p & i
# point should be i
answer = i.SqDist(p)
pt1 = geoalgo.Vector(3)
pt2 = geoalgo.Vector(3)
tim = time()
pt1_v = iAlgo.Intersection(box, lin)
intersectT_f += (time() - tim)
pt2_v = iAlgo.Intersection(lin, box)
if pt1_v.size(): pt1 = pt1_v[0]
if pt2_v.size(): pt2 = pt2_v[0]
a1 = pt1.SqDist(p)
a2 = pt2.SqDist(p)
if not (np.abs(answer - a1) < _epsilon): success_f = 0
if not (np.abs(answer - a2) < _epsilon): success_f = 0
for x in xrange(3):
if not (np.abs(pt1[x] - i[x]) < _epsilon): success_f = 0
if not (np.abs(pt1[x] - i[x]) < _epsilon): success_f = 0
totSuccess_f += success_f
# now backwards:
# make line go in opposite direction -> intersection point should be the same
d = p - i
lin = geoalgo.HalfLine(p, d)
pt1 = geoalgo.Vector(3)
pt2 = geoalgo.Vector(3)
tim = time()
pt1_v = iAlgo.Intersection(box, lin, 1)
intersectT_b += (time() - tim)
pt2_v = iAlgo.Intersection(lin, box, 1)
if pt1_v.size(): pt1 = pt1_v[0]
if pt2_v.size(): pt2 = pt2_v[0]
a1 = pt1.SqDist(p)
a2 = pt2.SqDist(p)
if not (np.abs(answer - a1) < _epsilon): success_b = 0
if not (np.abs(answer - a2) < _epsilon): success_b = 0
for x in xrange(3):
if not (np.abs(pt1[x] - i[x]) < _epsilon): success_b = 0
if not (np.abs(pt1[x] - i[x]) < _epsilon): success_b = 0
if (success_b == 1): totSuccess_b += 1
if (float(totSuccess_f) / tests < 1):
info(NO +
"Success: {0}%".format(100 * float(totSuccess_f) / tests) +
ENDC)
else:
info(OK +
"Success: {0}%".format(100 * float(totSuccess_f) / tests) +
ENDC)
info("Time for Intersection : {0:.3f} us".format(
1E6 * intersectT_f / tests))
if (float(totSuccess_b) / tests < 1):
info(NO +
"Success: {0}%".format(100 * float(totSuccess_b) / tests) +
ENDC)
else:
info(OK +
"Success: {0}%".format(100 * float(totSuccess_b) / tests) +
ENDC)
info("Time for Intersection : {0:.3f} us".format(
1E6 * intersectT_b / tests))
except Exception:
error('geoalgo::IntersectAlgo unit test failed.')
print traceback.format_exception(*sys.exc_info())[2]
return 1
info('geoalgo::IntersectAlgo unit test complete.')
return 0
import ROOT from ROOT import ertool from ROOT import geoalgo from basictool.geoviewer import GeoViewer import matplotlib.pyplot as plt import random as rnd import sys # # Make my fake data input # # Make a fake track and shower # shower1: shrDir = geoalgo.Vector(0.3,1.0,0.5)# direction shrStart = geoalgo.Vector(14,-10,30) # start point shower1 = ertool.Shower(shrStart,shrDir,20,5) shower1._energy = 100 shower1._dedx = 2.2 # shower2: shrDir = geoalgo.Vector(-0.33,1.0,0.5)# direction shrStart = geoalgo.Vector(10,-11,30) # start point shower2 = ertool.Shower(shrStart,shrDir,20,5) shower2._energy = 100 shower2._dedx = 2.2 # load viewer plt.ion() viewer = GeoViewer()
plt.ion() tottime = 0 # track: viewer2 = GeoViewer() viewer2._use_box = False geoAlgo = geoalgo.GeoAlgo() ntries = 1000 npoints = 800 #for x in xrange(ntries): track = ertool.Track() #viewer2._fig.clear() #for n in xrange(npoints): #time.sleep(0.01) point = geoalgo.Vector(rnd.random(), rnd.random(), rnd.random()) n = 0 viewer2.add(point, 'pt %i' % n, 'k') track.push_back(point) viewer2.add(point, 'pt %i' % n, 'k') track.push_back(point) viewer2.add(point, 'pt %i' % n, 'k') track.push_back(point) viewer2.add(point, 'pt %i' % n, 'k') track.push_back(point) t = time.time() sphere = geoAlgo.boundingSphere(track) tdelta = time.time() - t tottime += tdelta #print "sphere center is: [%.02f,%.02f,%.02f]"%(sphere.Center()[0],sphere.Center()[1],sphere.Center()[2]) #print "sphere radius is: %f"%sphere.Radius()
pt_e = retrj[b][len(retrj[b]) - 1]
dist = pt_s.Dist(pt_e)
if len(retrj[b]) < 15:
break
for bb in range(1, 15):
pt1 = retrj[b][bb]
pt2 = retrj[b][bb - 1]
dist = pt0.Dist(pt1)
weight = pt1.Dist(pt2)
line = geoalgo.LineSegment(pt1, pt2)
viewer.add(line, " ", "b")
pt_h = retrj[b][bb]
dist = pt_s.Dist(pt_h)
line0 = geoalgo.LineSegment(retrj[b][bb], retrj[b][bb + 1])
pt3 = geoalgo.Vector()
pt4 = geoalgo.Vector()
d = geoAlgo.SqDist(line0, mctrj[0], pt3, pt4)
pt5 = geoalgo.Vector((pt4[0] - pt3[0]) * weight + pt3[0],
(pt4[1] - pt3[1]) * weight + pt3[1],
(pt4[2] - pt3[2]) * weight + pt3[2])
print d
#line1 = geoalgo.LineSegment(pt3,pt4)
line1 = geoalgo.LineSegment(pt3, pt5)
viewer.add(line1, ' ', 'k')
viewer.construct()
'''
viewer._ax.set_xlim(-150,300)
viewer._ax.set_ylim(-120,350)
viewer._ax.set_zlim(-10,1070)
import ROOT
from ROOT import ertool
from ROOT import geoalgo
from basictool.geoviewer import GeoViewer
import matplotlib.pyplot as plt
#
# Make my fake data input
#
# Make a fake track and shower
# shower:
shrDir = geoalgo.Vector(0.3,1.0,0.5)# direction
shrStart = geoalgo.Vector(10,-10,30) # start point
shower = ertool.Shower(shrStart,shrDir,20,5)
shower._energy = 100
shower._dedx = 2.2
# track:
track = ertool.Track()
for n in xrange(11):
track.push_back(geoalgo.Vector(-45+(10+45)*(n/10.),15-(10+15)*(n/10.),4+(30-4)*(n/10.)))
track._energy = 435;
plt.ion()
viewer = GeoViewer()
viewer.add(shower,'shower','b')
viewer.add(track,'track','r')
viewer.show()
try:
counter = input('Hit ENTER when you are ready to close the viewer')
