def heading_to_point(
start_x: float,
start_y: float,
vel_x: float,
vel_y: float,
point_x: float,
point_y: float,
):
"""
Return a heading, in 2D RH coordinate system.
x,y: the current position of the object
vel_x, vel_y: the current velocity vector of motion for the object
point_x, point_y: the destination point to head towards
returns: offset angle in radians in the range [-pi .. pi]
where:
0.0: object is moving directly towards the point
[-pi .. <0]: object is moving to the "right" of the point
[>0 .. -pi]: object is moving to the "left" of the point
[-pi, pi]: object is moving directly away from the point
"""
# vector to point
dx = point_x - start_x
dy = point_y - start_y
# if the ball is already at the target location or
# is not moving, return a heading of 0 so we don't
# attempt to normalize a zero-length vector
if dx == 0 and dy == 0:
return 0
if vel_x == 0 and vel_y == 0:
return 0
# vectors and lengths
u = vector.normalize([dx, dy, 0.0])
v = vector.normalize([vel_x, vel_y, 0.0])
ul = vector.length(u)
vl = vector.length(v)
# no velocity? already on the target?
angle = 0.0
if (ul != 0.0) and (vl != 0.0):
# angle between vectors
uv_dot = vector.dot(u, v)
# signed angle
x = u[0]
y = u[1]
angle = math.atan2(vector.dot([-y, x, 0.0], v), uv_dot)
if math.isnan(angle):
angle = 0.0
return angle
def point_closest_point_on_line_segment( point, segment ):
"""Calculates the point on the line segment that is closest
to the specified point.
This is similar to point_closest_point_on_line, except this
is against the line segment of finite length. Whereas point_closest_point_on_line
checks against a line of infinite length.
:param numpy.array point: The point to check with.
:param numpy.array line_segment: The finite line segment to check against.
:rtype: numpy.array
:return: The closest point on the line segment to the point.
"""
# check if the line has any length
rl = segment[ 1 ] - segment[ 0 ]
squared_length = vector.squared_length( rl )
if squared_length == 0.0:
return segment[ 0 ]
rp = point - segment[ 0 ]
# check that / squared_length is correct
dot = vector.dot( rp, rl ) / squared_length;
if dot < 0.0:
return segment[ 0 ]
elif dot > 1.0:
return segment[ 1 ]
# within segment
# perform the same calculation as closest_point_on_line
return segment[ 0 ] + (rl * dot)
def batch():
vecs1 = numpy.array([
# adjacent
[ 1.0, 0.0, 0.0],
# parallel
[ 0.0, 1.0, 0.0 ],
# angled
[ 1.0, 1.0, 0.0 ]
])
vecs2 = numpy.array([
# adjacent
[ 0.0, 1.0, 0.0],
# parallel
[ 0.0, 1.0, 0.0 ],
# angled
[ 0.0, 1.0, 0.0 ]
])
result = vector.dot( vecs1, vecs2 )
expected = numpy.sum( vecs1 * vecs2, axis = -1 )
self.assertTrue(
numpy.array_equal( result, expected ),
"Vector dot calculation incorrect"
)
# assert individually
assert numpy.array_equal( result[ 0 ], numpy.sum( vecs1[ 0 ] * vecs2[ 0 ] ) )
assert numpy.array_equal( result[ 1 ], numpy.sum( vecs1[ 1 ] * vecs2[ 1 ] ) )
assert numpy.array_equal( result[ 2 ], numpy.sum( vecs1[ 2 ] * vecs2[ 2 ] ) )
def point_closest_point_on_line( point, line ):
"""Calculates the point on the line that is closest to
the specified point.
:param numpy.array point: The point to check with.
:param numpy.array line: The line to check against.
:rtype: numpy.array
:return: The closest point on the line to the point.
"""
"""
rl = va->b (relative line)
rp = va->p (relative point)
u' = u / |u| (normalise)
cp = a + (u' * (u'.v))
where:
a = line start
b = line end
p = point
cp = closest point
"""
rl = line[ 1 ] - line[ 0 ]
rp = point - line[ 0 ]
vector.normalise( rl )
dot = vector.dot( rl, rp )
return line[ 0 ] + (rl * dot)
def intersectRayUnitSphere(rayOri, rayDir):
# (t*rd + ro)^2 = 1
# t^2*<rd, rd> + 2*t*<rd, ro> + <ro, ro> - 1 = 0
# a := <rd, rd>, b := <rd, ro>, c := <ro, ro> - 1
# t := (b +- sqrt(b^2 - a*c)) / a
a = vector.dot(rayDir, rayDir)
b = vector.dot(rayDir, rayOri)
c = vector.dot(rayOri, rayOri) - 1
det = b*b - a*c
if det < 0:
return None
det = np.sqrt(det)
ts = [(-b + det) / a, (-b - det) / a]
ts = filter(lambda x: x >= 0, ts)
if ts == []:
return None
t = min(ts)
return rayOri + t*rayDir
def ray_intersect_plane( ray, plane, front_only = False ):
"""Calculates the intersection point of a ray and a plane.
:param numpy.array ray: The ray to test for intersection.
:param numpy.array plane: The ray to test for intersection.
:param boolean front_only: Specifies if the ray should
only hit the front of the plane.
Collisions from the rear of the plane will be
ignored.
:return The intersection point, or None
if the ray is parallel to the plane.
Returns None if the ray intersects the back
of the plane and front_only is True.
"""
"""
Distance to plane is defined as
t = (pd - p0.n) / rd.n
where:
rd is the ray direction
pd is the point on plane . plane normal
p0 is the ray position
n is the plane normal
if rd.n == 0, the ray is parallel to the
plane.
"""
p = plane[ :3 ] * plane[ 3 ]
n = plane[ :3 ]
rd_n = vector.dot( ray[ 1 ], n )
if rd_n == 0.0:
return None
if front_only == True:
if rd_n >= 0.0:
return None
pd = vector.dot( p, n )
p0_n = vector.dot( ray[ 0 ], n )
t = (pd - p0_n) / rd_n
return ray[ 0 ] + (ray[ 1 ] * t)
def dot( quat1, quat2 ):
"""Calculate the dot product of quaternions.
:param numpy.array quat1: The first quaternion(s).
:param numpy.array quat2: The second quaternion(s).
:rtype: float, numpy.array
:return: If a 1d array was passed, it will be a scalar.
Otherwise the result will be an array of scalars with shape
vec.ndim with the last dimension being size 1.
"""
return vector.dot( quat1, quat2 )
def angle():
vec1 = numpy.array( [ 1.0, 1.0, 0.0 ] )
vec2 = numpy.array( [ 0.0, 1.0, 0.0 ] )
result = vector.dot( vec1, vec2 )
expected = numpy.sum( vec1 * vec2 )
assert numpy.array_equal( expected, vec_dot( vec1, vec2 ) )
self.assertTrue(
numpy.array_equal( result, expected ),
"Dot product of angled vectors incorrect"
)
def point_closest_point_on_ray( point, ray ):
"""Calculates the point on a ray that is closest to a point.
:param numpy.array point: The point to check with.
:param numpy.array ray: The ray to check against.
:rtype: numpy.array
:return: The closest point on the ray to the point.
"""
"""
t = (p - rp).n
cp = rp + (n * t)
where
p is the point
rp is the ray origin
n is the ray normal of unit length
t is the distance along the ray to the point
"""
normalised_n = vector.normalise( ray[ 1 ] )
relative_point = (point - ray[ 0 ])
t = vector.dot( relative_point, normalised_n )
return ray[ 0 ] + ( normalised_n * t )
def point_intersect_line_segment( point, line ):
"""Calculates the intersection point of a point and a line segment.
Performed by checking if the cross-product
of the point relative to the line is
0 and if the dot product of the point
relative to the line start AND the end
point relative to the line start is
less than the segment's squared length.
"""
rl = line[ 1 ] - line[ 0 ]
rp = point - line[ 0 ]
cross = vector.cross( rl, rp )
dot = vector.dot( rp, rl )
squared_length = vector.squared_length( rl )
if numpy.count_nonzero( cross ) > 0:
return None
if \
dot < 0.0 or \
dot > squared_length:
return None
return point
def dot(self, other):
return vector.dot(self, type(self)(other))
def dot(self, other):
return vector.dot(self, type(self)(other))
