def get_intersection(self, segment):
"""
Calculate the intersection of two line segments
"""
_lhs = TupleMath.subtract(self.end, self.start)[0:2]
_lhs = TupleMath.multiply(_lhs, (1.0, -1.0))
_lhs += (sum(TupleMath.multiply(_lhs, self.start)),)
_rhs = TupleMath.subtract(segment.end, segment.start)[0:2]
_rhs = TupleMath.multiply(_lhs, (1.0, -1.0))
_rhs += (sum(TupleMath.multiply(_rhs, segment.start)),)
_determinant = TupleMath.cross(_lhs, _rhs, (0, 0, 1))[2]
if not _determinant:
return (math.nan, math.nan)
_intersection = (
TupleMath.cross(_lhs, _rhs, (1, 0, 0))[0],
TupleMath.cross(_lhs, _rhs, (0, 1, 0))[1]
)
return TupleMath.scale(_intersection, 1.0 / _determinant)
def get_ortho_vector(line, distance, side=''):
"""
Return the orthogonal vector pointing toward the indicated side at the
provided position. Defaults to left-hand side
"""
_dir = 1.0
_side = side.lower()
if _side in ['r', 'rt', 'right']:
_dir = -1.0
start = tuple(line.get('Start'))
end = tuple(line.get('End'))
bearing = line.get('BearingIn')
if (start is None) or (end is None):
return None, None
_delta = TupleMath.subtract(end, start)
_delta = TupleMath.normalize(_delta)
_left = tuple(-_delta.y, _delta.x, 0.0)
_coord = get_coordinate(start, bearing, distance)
return _coord, TupleMath.multiply(_left, _dir)
def on_drag(self, user_data):
_xlate = user_data.matrix.getValue()[3]
_point = Drag.drag_tracker.drag_position
for _v in ['start', 'center', 'end']:
if _v in user_data.obj.name:
self.arc.set(_v, _point)
elif _v == 'center' and 'arc' in user_data.obj.name:
if self.drag_axis:
_xlate = TupleMath.project(_xlate[0:3], self.drag_axis)
self.arc.set(_v, TupleMath.add(self.drag_start_point, _xlate))
else:
self.arc.set(_v, None)
self.arc.radius = None
self.arc.tangent = None
self.update()
for _cb in self.on_drag_callbacks:
_cb(user_data)
def set_step(self, step, force_refresh=False):
"""
Set the absolulte position of the vehicle along it's path
"""
if not self.path:
return
_num_segs = len(self.path.segments) - 1
if step > _num_segs:
step = _num_segs
if self.step == step and not force_refresh:
return
self.orientation = \
-TupleMath.bearing(
self.path.segments[step].vector, (1.0, 0.0, 0.0)
)
_ori = \
-TupleMath.signed_bearing(
self.path.segments[step].vector, (1.0, 0.0, 0.0)
)
if self.step < _num_segs:
_angle = self.path.segments[step].angle
if _angle:
self.update(_angle)
self.step = step
def _flip_reversed_edges(self, dct):
"""
Reverse the points of flipped edges in the geometry dictionary
"""
_keys = tuple(dct.keys())
_prev = _keys[0]
_flip_test = []
_flipped_edges = {}
#check for proper point order
for _edge in _keys[1:]:
#test for flipped endpoints
_flip_test = [
TupleMath.manhattan(_prev.StartPoint, _edge.StartPoint)[0] < 0.01,
TupleMath.manhattan(_prev.EndPoint, _edge.EndPoint)[0] < 0.01,
TupleMath.manhattan(_prev.StartPoint, _edge.EndPoint)[0] < 0.01
]
#flip the current edge points if either of last two cases
if _flip_test[1] or _flip_test[2]:
if _edge not in _flipped_edges:
_flipped_edges[_edge] = True
dct[_edge] = dct[_edge][::-1]
#flip the previous edge points if first or last case
if _flip_test[0] or _flip_test[2]:
if _prev not in _flipped_edges:
_flipped_edges[_prev] = True
dct[_prev] = dct[_prev][::-1]
_prev = _edge
def project(self, displacement):
"""
Project the point along the axis vector from the center
displacement - distance from center
"""
return TupleMath.add(self.center,
TupleMath.scale(self.vector, displacement))
def set_look_ahead(self, vector):
"""
Set the angle between the segment vector and passed vector
vector - a unit vecotr in tuple form
"""
self.angle = -TupleMath.signed_bearing(vector, self.vector)
self.tangent = TupleMath.add(vector, self.vector)
def get_coordinate(start, bearing, distance):
"""
Return the x/y coordinate of the line at the specified distance along it
"""
_vec = TupleMath.bearing_vector(bearing)
return TupleMath.add(tuple(start),
TupleMath.multiply(tuple(_vec), distance))
def __init__(self, previous, current):
"""
Constructor
"""
self.position = previous
self.vector = TupleMath.unit(TupleMath.subtract(current, previous))
self.angle = 0.0
self.tangent = self.vector
def set_length(self, length):
"""
Set the axis length and update the axis end points
"""
self.length = length
_half_vector = TupleMath.scale(self.vector, self.length / 2.0)
#set left (ccw) and right (cw) end points, respectively...
self.end_points = (TupleMath.add(self.center, _half_vector),
TupleMath.subtract(self.center, _half_vector))
def get_parameters(line):
"""
Return a fully-defined line
"""
_result = line
if isinstance(line, dict):
_result = Line(line)
_coord_truth = [_result.start, _result.end]
_param_truth = [not math.isnan(_result.bearing), _result.length > 0.0]
#both coordinates defined
_case_one = all(_coord_truth)
#only one coordinate defined, plus both length and bearing
_case_two = any(_coord_truth) \
and not all(_coord_truth) \
and all(_param_truth)
if _case_one:
line_vec = TupleMath.subtract(_result.end, _result.start)
_length = TupleMath.length(line_vec)
if _result.length:
if support.within_tolerance(_result.length, _length):
_length = _result.length
_result.length = _length
elif _case_two:
_vec = \
support.vector_from_angle(_result.bearing).multiply(_result.length)
if _result.start:
_result.end = TupleMath.add(_result.start, _vec)
else:
_result.start = TupleMath.add(_result.end, _vec)
else:
print('Unable to calculate parameters for line', _result)
#result = None
#if _case_one or _case_two:
# result = {**{'Type': 'Line'}, **line}
return _result
def validate_stationing(self):
"""
Iterate the geometry, calculating the internal start station
based on the actual station and storing it in an
'InternalStation' parameter tuple for the start
and end of the curve
"""
prev_station = self.data.get('meta').get('StartStation')
prev_coord = self.data.get('meta').get('Start')
if (prev_coord is None) or (prev_station is None):
return
for _geo in self.data.get('geometry'):
if not _geo:
continue
_geo['InternalStation'] = None
geo_station = _geo.get('StartStation')
geo_coord = _geo.get('Start')
#if no station is provided, try to infer it from the start
#coordinate and the previous station
if geo_station is None:
geo_station = prev_station
if not geo_coord:
geo_coord = prev_coord
print(geo_coord, prev_coord)
delta = TupleMath.length(
TupleMath.subtract(tuple(geo_coord), tuple(prev_coord)))
if not support.within_tolerance(delta):
geo_station += delta / units.scale_factor()
if _geo.get('Type') == 'Line':
_geo.start_station = geo_station
else:
_geo['StartStation'] = geo_station
prev_coord = _geo.get('End')
prev_station = _geo.get('StartStation') \
+ _geo.get('Length')/units.scale_factor()
int_sta = self.get_internal_station(geo_station)
_geo['InternalStation'] = (int_sta, int_sta + _geo.get('Length'))
def get_bearings(pc, pt, pi, ctr, terminate_early=False):
"""
Return the bearings of the vectors associated with the points
"""
_fv = lambda x, y: TupleMath.subtract(x, y) if x and y else None
_vecs = [
_fv(pi, pc),
_fv(pt, pi),
_fv(pc, ctr),
_fv(pt, ctr),
_fv(pi, ctr),
_fv(pt, pc)
]
_fa = lambda x, y: TupleMath.angle_between(x, y) if x and y else 0.0
_ang = [
_fa(_vecs[0], _vecs[1]),
_fa(_vecs[2], _vecs[3]),
_fa(_vecs[4], _vecs[3]) * 2,
_fa(_vecs[4], _vecs[2]) * 2,
_fa(_vecs[5], _vecs)
]
_pts = []
_fvec = lambda x, y: TupleMath.subtract(x, y) if x and y else None
_vecs = [
_fvec(pi, pc),
_fvec(pt, pi),
_fvec(pc, ctr),
_fvec(pt, ctr),
_fvec(pi, ctr)
]
_fang = lambda x, y: TupleMath.angle_between(x, y) if x and y else None
_deltas = [
_fang(_vecs[0], _vecs[1]),
_fang(_vecs[2], _vecs[3]),
_fang(_vecs[4], _vecs[3]),
_fang(_vecs[4], _vecs[2])
]
def _update_text(self):
"""
Update curve-specific text
"""
self.text.set_translation(TupleMath.mean(self.coordinates))
self.text_center = self.center
self.set_text_translation((0.0, 0.0, 0.0))
def setup_drag_references(self, nam):
"""
Set parameters to be referenced during on_drag operations
"""
_drag_ctr = self.arc.start
_bearing = self.arc.bearing_in
_origin = None
if 'center' in nam or 'arc' in nam:
_drag_ctr = self.arc.center
_bearing = TupleMath.bearing(
TupleMath.subtract(tuple(self.arc.pi), tuple(self.arc.center)))
if 'arc' in nam:
_l = len(self.arc.points)
_l_2 = int(_l / 2)
_drag_ctr = self.arc.points[_l_2 - 1]
if (_l % 2):
_drag_ctr = TupleMath.mean(_drag_ctr,
self.arc.points[_l_2])
_drag_ctr = self.arc.center
self.drag_start_point = self.arc.center
_origin = self.arc.center
elif 'end' in nam:
_drag_ctr = self.arc.end
_bearing = self.arc.bearing_out
self.drag_center = _drag_ctr
if not _origin:
_origin = _drag_ctr
#generate constraint geometry along an axis defined by the bearing
while _bearing > math.pi:
_bearing -= math.pi
self.drag_axis = (1.0, 1.0 / math.tan(_bearing), 0.0)
Drag.drag_tracker.set_constraint_geometry(self.drag_axis, _origin)
def __init__(self, start_point, end_point):
"""
Constructor
"""
self.start = start_point
self.end = end_point
self.points = (self.start, self.end)
self.vector = TupleMath.subtract(self.end, self.start)
self.box = self.build_bounding_box()
def zero_reference_coordinates(self):
"""
Reference the coordinates to the start point
by adjustuing by the datum
"""
datum = self.get_datum()
for _geo in self.data.get('geometry'):
for _key in ['Start', 'End', 'Center', 'PI']:
if _geo.get(_key) is None:
continue
_geo[_key] = TupleMath.subtract(_geo[_key], datum)
if self.data.get('meta').get('End'):
self.data.get('meta')['End'] = \
TupleMath.subtract(self.data.get('meta').get('End'), datum)
def set_vector(self, vector):
"""
Set the vector of the axis, converting it to unit length
Also calculates axis end points
"""
if vector:
vector = TupleMath.unit(vector)
self.vector = vector
if not self.vector:
return
def validate_curve_drag(self, user_data):
"""
Validates the changes to the curves, noting errors when
curves overlap or exceed tangent lengths
"""
_prev_curve = None
_curve = None
_max = len(self.curve_trackers)
_pp = self.drag_points[0]
_lines = []
for _p in self.drag_points[1:]:
_lines.append(TupleMath.length(_p, _pp))
_pp = _p
for _i, _l in enumerate(_lines):
_is_invalid = False
#the last segment doesn't need _prev_curve
if _i < _max:
_curve = self.curve_trackers[_i]
else:
#abort if last curve has already been marked invalid
if _curve.is_invalid:
return
_prev_curve = None
#calculate sum of tangnets
_tan = _curve.arc.tangent
if _prev_curve:
_tan += _prev_curve.arc.tangent
#curve tangents must not exceed segment length
_is_invalid = _tan > _l
#invalidate accordingly.
_curve.is_invalid = _is_invalid
if _prev_curve and not _prev_curve.is_invalid:
_prev_curve.is_invalid = _is_invalid
_prev_curve = _curve
def set_position(self, position=None):
"""
Set the widget position.
Position is a 2-tuple relative to GUI view X/Y coordinates
"""
if not position:
position = self.position
self.position = position[:2]
_pos = TupleMath.add(position, self.offset)
self.move(position[0], position[1])
def _combine_points(self, dct):
"""
Combine a dictionary of points into a single list,
eliminating duplicates and building the vector / angle tuples
"""
_points = []
for _v in dct.values():
_p = [_w for _w in _v]
#Eliminate duplicates at start / end points
if _points:
if TupleMath.manhattan(_p[0], _points[-1]) < 0.01:
_p = _p[1:]
_points += _p
return _points
def is_intersecting(self, line_segment, get_point=True):
"""
Test to see if two line segments are interescting
--------
Algorithm designed by Simon Walton, Dept. of Computer Sciene, Swansea
http://cs.swan.ac.uk/~cssimon/line_intersection.html
"""
_y43 = line_segment.vector[1]
_x43 = line_segment.vector[0]
_y21 = self.vector[1]
_x21 = self.vector[0]
_x31 = line_segment.start[0] - self.start[0]
_y31 = line_segment.start[1] - self.start[1]
_d = ((_x43*-_y21)-(-_x21*_y43))
_u = ((-_y43*-_x31) + (_x43*-_y31)) / _d
_v = ((-_y21*-_x31) + (_x21*-_y31)) / _d
_is_true = (0.0 <= _u <= 1.0) and (0.0 <= _v <= 1.0)
_data = ()
if _is_true:
if get_point:
_data = TupleMath.add(self.start, (_u*_x21, _u*_y21)) + (0.0,)
else:
_data = (
(_x21, _y21),
(_x43, _y43),
(_u, _v)
)
else:
_data = _u, _v
return (_is_true, _data, (_u, _v))
def rebuild_bearings(self, matrix, pi_nums):
"""
Recaluclate bearings and update curves accordingly
"""
_xlate = matrix.getValue()[3]
if _xlate == self.last_update:
return
if not pi_nums:
return
self.last_update=_xlate
_pi = SimpleNamespace(
start=0, end=0, points=self.alignment_tracker.points.copy(),
bearings=[], count=len(self.alignment_tracker.points))
_pi.start = min(pi_nums)
_pi.end = max(pi_nums) + 1
_p = self.view_state.transform_points(
_pi.points[_pi.start:_pi.end], matrix)
#apply translation to selected PI's
#for _i in range(_pi.start, _pi.end):
# _pi.points[_i] = TupleMath.add(_pi.points[_i], _xlate)
_pi.points[_pi.start:_pi.end] = _p
#get range of PI's for bearing calcs
_pi.start = max(_pi.start - 2, 0)
_pi.end = min(_pi.end + 2, _pi.count)
_pi.points = _pi.points[_pi.start:_pi.end]
_pi.count = len(_pi.points)
for _i, _p in enumerate(_pi.points):
_b = SimpleNamespace(inb = None, outb = None)
if _p is None:
_pi.bearings.append(_b)
continue
#calc inbound / outbound bearings
if _i < _pi.count - 1:
_b.outb = TupleMath.bearing(
TupleMath.subtract(_pi.points[_i + 1], _p))
if _i > 0:
_b.inb = TupleMath.bearing(
TupleMath.subtract(_p, _pi.points[_i - 1]))
_pi.bearings.append(_b)
_curve = SimpleNamespace(
start=max(_pi.start, 0), end=_pi.end - 1)
for _i, _c in enumerate(self.curve_trackers[_curve.start:_curve.end]):
_b = _pi.bearings[_i + 1]
_c.set_pi(_pi.points[_i + 1])
_c.set_bearings(_b.inb, _b.outb)
_c.update()
self.drag_points = _pi.points
def discretize_geometry(self, interval=None, method='Segment', delta=10.0):
"""
Discretizes the alignment geometry to a series of vector points
interval - the starting internal station and length of curve
method - method of discretization
delta - discretization interval parameter
"""
geometry = self.data.get('geometry')
points = []
last_curve = None
#undefined = entire length
if not interval:
interval = [0.0, self.data.get('meta').get('Length')]
#only one element = starting position
if len(interval) == 1:
interval.append(self.data.get('meta').get('Length'))
#discretize each arc in the geometry list,
#store each point set as a sublist in the main points list
for curve in geometry:
if not curve:
continue
_sta = curve.get('InternalStation')
#skip if curve end precedes start of interval
if _sta[1] < interval[0]:
continue
#skip if curve start exceeds end of interval
if _sta[0] > interval[1]:
continue
_start = _sta[0]
#if curve starts before interval, use start of interval
if _sta[0] < interval[0]:
_start = interval[0]
_end = _sta[1]
#if curve ends past the interval, stop at end of interval
if _sta[1] > interval[1]:
_end = interval[1]
#calculate start position on arc and length to discretize
_arc_int = [_start - _sta[0], _end - _start]
#just in case, skip for zero or negative lengths
if _arc_int[1] <= 0.0:
continue
if curve.get('Type') == 'Curve':
_pts = arc.get_points(curve,
size=delta,
method=method,
interval=_arc_int)
if _pts:
points.append(_pts)
elif curve.get('Type') == 'Spiral':
_pts = spiral.get_points(curve, size=delta, method=method)
if _pts:
points.append(_pts)
else:
_start_coord = line.get_coordinate(curve.get('Start'),
curve.get('BearingIn'),
_arc_int[0])
points.append([
_start_coord,
line.get_coordinate(_start_coord, curve.get('BearingIn'),
_arc_int[1])
])
last_curve = curve
#store the last point of the first geometry for the next iteration
_prev = points[0][-1]
result = points[0]
if not (_prev and result):
return None
#iterate the point sets, adding them to the result set
#and eliminating any duplicate points
for item in points[1:]:
_v = item
#duplicate points are within a hundredth of a foot of each other
if TupleMath.length(
TupleMath.subtract(tuple(_prev), tuple(item[0]))) < 0.01:
_v = item[1:]
result.extend(_v)
_prev = item[-1]
#add a line segment for the last tangent if it exists
last_tangent = abs(
self.data.get('meta').get('Length') \
- last_curve.get('InternalStation')[1]
)
if not support.within_tolerance(last_tangent):
_vec = TupleMath.bearing_vector(
last_curve.get('BearingOut') * last_tangent)
# _vec = support.vector_from_angle(last_curve.get('BearingOut'))\
# .multiply(last_tangent)
last_point = tuple(result[-1])
result.append(TupleMath.add(last_point, _vec))
#set the end point
if not self.data.get('meta').get('End'):
self.data.get('meta')['End'] = result[-1]
return result
def validate_coordinates(self, zero_reference):
"""
Iterate the geometry, testing for incomplete / incorrect station /
coordinate values. Fix them where possible, error otherwise
"""
#calculate distance between curve start and end using
#internal station and coordinate vectors
_datum = self.data.get('meta')
_geo_data = self.data.get('geometry')
_prev_geo = {
'End': _datum.get('Start'),
'InternalStation': (0.0, 0.0),
'StartStation': _datum.get('StartStation'),
'Length': 0.0
}
if zero_reference:
_prev_geo['End'] = Vector()
for _geo in _geo_data:
if not _geo:
continue
#get the vector between the two geometries
#and the station distance
_vector = TupleMath.subtract(tuple(_geo.get('Start')),
tuple(_prev_geo.get('End')))
_sta_len = abs(
_geo.get('InternalStation')[0] \
- _prev_geo.get('InternalStation')[1]
)
#calculate the difference between the vector length
#and station distance in document units
_delta = \
(TupleMath.length(_vector) - _sta_len) / units.scale_factor()
#if the stationing / coordinates are out of tolerance,
#the error is with the coordinate vector or station
if not support.within_tolerance(_delta):
bearing_angle = TupleMath.bearing(_vector)
#fix station if coordinate vector bearings match
if support.within_tolerance(bearing_angle,
_geo.get('BearingIn')):
_int_sta = (
_prev_geo.get('InternalStation')[1] \
+ TupleMath.length(_vector),
_geo.get('InternalStation')[0]
)
_start_sta = _prev_geo.get('StartStation') + \
_prev_geo.get('Length') / \
units.scale_factor() + \
TupleMath.length(_vector) / \
units.scale_factor()
_geo['InternalStation'] = _int_sta
_geo['StartStation'] = _start_sta
#otherwise, fix the coordinate
else:
_bearing_vector = TupleMath.multiply(
TupleMath.bearing_vector(_geo.get('BearingIn')),
_sta_len)
_start_pt = TupleMath.add(_prev_geo.get('End'),
_bearing_vector)
_geo['Start'] = _start_pt
_prev_geo = _geo
def validate_datum(self):
"""
Ensure the datum is valid, assuming 0+00 / (0,0,0)
for station and coordinate where none is suplpied and it
cannot be inferred fromt the starting geometry
"""
_datum = self.data.get('meta')
_geo = self.data.get('geometry')[0]
if not _geo or not _datum:
return
_datum_truth = [
not _datum.get('StartStation') is None,
not _datum.get('Start') is None
]
_geo_truth = [
not _geo.get('StartStation') is None, not _geo.get('Start') is None
]
#----------------------------
#CASE 0
#----------------------------
#both defined? nothing to do
if all(_datum_truth):
return
#----------------------------
#Parameter Initialization
#----------------------------
_geo_station = 0
_geo_start = Vector()
if _geo_truth[0]:
_geo_station = _geo.get('StartStation')
if _geo_truth[1]:
_geo_start = _geo.get('Start')
#---------------------
#CASE 1
#---------------------
#no datum defined? use initial geometry or zero defaults
if not any(_datum_truth):
_datum['StartStation'] = _geo_station
_datum['Start'] = _geo_start
return
#--------------------
#CASE 2
#--------------------
#station defined?
#if the geometry has a station and coordinate,
#project the start coordinate
if _datum_truth[0]:
_datum['Start'] = _geo_start
#assume geometry start if no geometry station
if not _geo_truth[0]:
return
#scale the distance to the system units
delta = _geo_station - _datum['StartStation']
#cutoff if error is below tolerance
if not support.within_tolerance(delta):
delta *= units.scale_factor()
else:
delta = 0.0
#assume geometry start if station delta is zero
if delta:
#calculate the start based on station delta
_datum['Start'] =\
TupleMath.subtract(_datum.get('Start'),
TupleMath.scale(
TupleMath.bearing_vector(_geo.get('BearingIn')),
delta)
#_geo.get('BearingIn')).multiply(delta)
)
return
#---------------------
#CASE 3
#---------------------
#datum start coordinate is defined
#if the geometry has station and coordinate,
#project the start station
_datum['StartStation'] = _geo_station
#assume geometry station if no geometry start
if _geo_truth[1]:
#scale the length to the document units
delta = TupleMath.length(
TupleMath.subtract(
_geo_start, _datum.get('Start'))) / units.scale_factor()
_datum['StartStation'] -= delta
def validate_alignment(self):
"""
Ensure the alignment geometry is continuous.
Any discontinuities (gaps between end / start coordinates)
must be filled by a completely defined line
"""
_prev_sta = 0.0
_prev_coord = self.data['meta']['Start']
_geo_list = []
#iterate through all geometry, looking for coordinate gaps
#and filling them with line segments.
for _geo in self.data.get('geometry'):
if not _geo:
continue
_coord = _geo.get('Start')
_d = abs(
TupleMath.length(
TupleMath.subtract(tuple(_coord), tuple(_prev_coord))))
#test for gap at start of geometry and end of previous geometry
if not support.within_tolerance(_d, tolerance=0.01):
#build the line using the provided parameters and add it
_geo_list.append(
line.get_parameters({
'Start':
Vector(_prev_coord),
'End':
Vector(_coord),
'StartStation':
self.get_alignment_station(_prev_sta),
'Bearing':
_geo.get('BearingIn'),
}).to_dict())
_geo_list.append(_geo)
_prev_coord = _geo.get('End')
_prev_sta = _geo.get('InternalStation')[1]
_length = 0.0
#fill in the alignment length. If the end of the alignment falls short
#of the calculated length, append a line to complete it.
if not self.data.get('meta').get('Length'):
_end = self.data.get('meta').get('End')
#abort - no overall length or end coordinate specified
if not _end:
return False
_prev = _geo_list[-1]
if TupleMath.length(TupleMath.subtract(_end,
_prev.get('End'))) > 0.0:
_geo_list.append(
line.get_parameters({
'Start':
_prev.get('End'),
'End':
_end,
'StartStation':
self.get_alignment_station(
_prev['InternalStation'][0]),
'Bearing':
_prev.get('BearingOut')
}).to_dict())
self.data.get('meta')['Length'] = 0.0
#accumulate length across individual geometry and test against
#total alignment length
for _geo in _geo_list:
_length += _geo.get('Length')
align_length = self.data.get('meta').get('Length')
if not support.within_tolerance(_length, align_length):
if _length > align_length:
self.data.get('meta')['Length'] = align_length
else:
_start = _geo_list[-1].get('End')
bearing = _geo_list[-1].get('BearingOut')
_end = line.get_coordinate(_start, bearing,
align_length - _length)
_geo_list.append(
line.get_parameters({
'Start':
_start,
'End':
_end,
'StartStation':
self.get_alignment_station(_geo['InternalStation'][1]),
'BearingOut':
bearing
}).to_dict())
self.data['geometry'] = _geo_list
return True
def update(self, angle):
"""
Update the vehicle position using the given steering angle (radians)
"""
# The angle subtended by the radius of the arc on which the front and
# center wheel midpoints lie is equal to the steering angle.
#
# The radius is the distance between the axles divided by
# the tangent of the steering angle
#
# The wheel angles are the arctangent of the axle distance divided by
# the radius, offset by half the vehicle width.
#
# The arc direction is -cw / +ccw
#
# If the vehicle is towed (self.lead_vehicle is not None), angle is
# ignored and calculations are performed using the lead vehicle
# turning radius.
_half_pi = math.pi / 2.0
if abs(angle) > self.maximum_angle:
return False
self.axis.angle = angle
self.radius = self.axle_distance / math.tan(angle)
#sign of angle to add / subtract from central steering angle.
#relative to ccw-oriented (left-hand) wheel
_sign = 1.0 # math.copysign(1.0, angle)
#get the index of the axle at the rear of the vehicle
#(negative distance from the center)
_back_axle = self.axle_dists.index(min(self.axle_dists))
#get the axle centerpoint
_back_center = self.axles[_back_axle].center
#get the unit orthogonal of the back axle axis
_back_vector = self.axles[_back_axle].ortho(_sign > 0.0)
self.center = TupleMath.add(
_back_center, TupleMath.scale(_back_vector, self.radius * -_sign))
#iterate each wheel pair. Left wheel is first in pair
for _axle in self.axles:
if _axle.is_fixed:
continue
#The wheel angle is the extra angle for each wheel
#added to the central steering angle
_wheel_angles = (_sign * self.axle_distance /
(self.radius + _axle.length / 2.0),
_sign * self.axle_distance /
(self.radius - _axle.length / 2.0))
_axle.wheels[0].angle = math.atan(_wheel_angles[0])
_axle.wheels[1].angle = math.atan(_wheel_angles[1])
self.angle = angle
return True