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 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_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 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 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