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 single_vector():
vec = numpy.array( [ 1.0, 1.0, 1.0 ] )
result = vector.squared_length( vec )
expected = numpy.sum( vec ** 2 )
self.assertEqual(
result,
expected,
"Vector squared length incorrect"
)
def squared_length( quat ):
"""Calculates the squared length of a quaternion.
Useful for avoiding the performanc penalty of
the square root function.
:param numpy.array quat: The quaternion to measure.
: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.squared_length( quat )
def batch_square_length():
vec = numpy.array( [ 1.0, 1.0, 1.0 ] )
batch = numpy.tile( vec, (3,1) )
result = vector.squared_length( batch )
expected = numpy.array(
[ numpy.sum( vec ** 2 ) ]
)
expected = numpy.tile( expected, (3) )
self.assertTrue(
numpy.array_equal( result, expected ),
"Vector squared length calculation incorrect"
)
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 squared_distance_to(self, other) -> float:
return vector.squared_length(self.vector_to(other))
