def test_check_vector():
"""Test the check_vector function of the validation module.
"""
valid_params = [
(1, 2, 3),
(0, 0, 0),
(1.0, 0.5, 10988),
(-4.5, 0, 7.54)
]
expect_type_errors = [
"test",
3,
4.5,
[1, 2, 3],
("4", 5, 6)
]
expect_value_errors = [
(1, 2, 3, 4),
(1, 2),
(float("nan"), 2, 3),
(float("inf"), 2, 3),
(float("-inf"), 2, 3)
]
for param in valid_params:
result = validation.check_vector(param)
assert isinstance(result, tuple) and len(result) == 3
for param in expect_type_errors:
with pytest.raises(TypeError):
validation.check_vector(param)
for param in expect_value_errors:
with pytest.raises(ValueError):
validation.check_vector(param)
def calc_dot_product(vec1, vec2):
"""Calculate the dot product of two 3D vectors.
Args:
vec1 (tuple (float, float, float)): The first 3D vector
vec2 (tuple (float, float, float)): The second 3D vector
Returns:
float: The dot product of the two 3D vectors
"""
checked_vec1 = check_vector(vec1)
checked_vec2 = check_vector(vec2)
return (checked_vec1[0] * checked_vec2[0] +
checked_vec1[1] * checked_vec2[1] +
checked_vec1[2] * checked_vec2[2])
def sum_vectors(vec1, vec2):
"""Calculate the sum of two vectors.
Args:
vec1 (tuple (float, float, float)): The first 3D vector
vec2 (tuple (float, float, float)): The second 3D vector
Returns:
tuple (float, float, float): The sum of both vectors
"""
checked_vec1 = check_vector(vec1)
checked_vec2 = check_vector(vec2)
return (checked_vec1[0] + checked_vec2[0],
checked_vec1[1] + checked_vec2[1],
checked_vec1[2] + checked_vec2[2])
def calc_cross_product(vec1, vec2):
"""Calculate the cross product of two 3D vectors.
Args:
vec1 (tuple (float, float, float)): The first 3D vector
vec2 (tuple (float, float, float)): The second 3D vector
Returns:
tuple (float, float, float): The cross product of the two 3D vectors
"""
checked_vec1 = check_vector(vec1)
checked_vec2 = check_vector(vec2)
return (checked_vec1[1] * checked_vec2[2] -
checked_vec1[2] * checked_vec2[1],
checked_vec1[2] * checked_vec2[0] -
checked_vec1[0] * checked_vec2[2],
checked_vec1[0] * checked_vec2[1] -
checked_vec1[1] * checked_vec2[0])
def calc_magnitude(vec):
"""Calculate the magnitude of a 3D vector.
Args:
vec (tuple (float, float, float)): The 3D vector whose magnitude shall be calculated
Returns:
float: The magnitude of the 3D vector
"""
checked_vec = check_vector(vec)
return math.sqrt(checked_vec[0]**2 + checked_vec[1]**2 + checked_vec[2]**2)
def scale_vector(vec, scale):
"""Multiply each vector element with the scale factor.
Args:
vec (tuple (float, float, float)): The vector that shall be scaled
scale (float): The scale factor
Returns:
tuple (float, float, float): The scaled vector
"""
checked_vec = check_vector(vec)
checked_scale = check_geometry_parameter(scale)
return tuple([checked_scale * vec_elem for vec_elem in checked_vec])
def from_normal_form(cls, normal_vec, position_vec, radius):
"""Factory method to create a plane when having the plane
parameters in the plane's normal form.
Args:
normal_vec (tuple (float, float, float)): The normal vector of the plane
position_vec (tuple (float, float, float)): An arbitrary point on the plane.
That point is also used as the center point for the plane point creation radius
radius (float): The plane point creation radius
Returns:
Plane: The plane object
"""
n_vec = check_direction_vector(normal_vec)
n0_vec = normalize_vector(n_vec)
ref_point = check_vector(position_vec)
d_origin = calc_dot_product(n0_vec, ref_point)
return cls(normal_vec, d_origin, ref_point, radius)
def rotate_vector(vec, axis, angle):
"""Rotate a vector around an axis by an angle (radiant).
Args:
vec (tuple (float, float, float)): The vector that shall be rotated
axis (tuple (float, float, float)): The rotation axis
angle (float): The rotation angle (radiant) for a right-handed rotation
Returns:
tuple (float, float, float): The rotated vector
"""
(vec_x, vec_y, vec_z) = check_vector(vec)
(quat_w, quat_x, quat_y, quat_z) = get_as_rotation_quaternion(axis, angle)
# rotation of a vector p via a quaternion q is defined as: q * p * (q*)
qpq = reduce(multiply_quaternions, [(0.0, vec_x, vec_y, vec_z),
(quat_w, -quat_x, -quat_y, -quat_z)],
(quat_w, quat_x, quat_y, quat_z))
return (qpq[1], qpq[2], qpq[3])
def calc_perpendicular_vector(vec):
"""Calculate an arbitrary perpendicular vector.
Args:
vec (tuple (float, float float)): The 3D vector for which an arbitrary
perpendicular vector shall be calculated
Returns:
tuple (float, float, float): The perpendicular vector
"""
checked_vec = check_vector(vec)
magnitude = calc_magnitude(checked_vec)
if math.isclose(magnitude, 0.0, abs_tol=0.000001):
raise ValueError("""Invalid vector. Expected a vector with
a magnitude greater than zero""")
if math.fabs(checked_vec[0]) < math.fabs(checked_vec[1]) and \
math.fabs(checked_vec[0]) < math.fabs(checked_vec[2]):
return calc_cross_product((1.0, 0.0, 0.0), checked_vec)
elif math.fabs(checked_vec[1]) < math.fabs(checked_vec[2]):
return calc_cross_product((0.0, 1.0, 0.0), checked_vec)
return calc_cross_product((0.0, 0.0, 1.0), checked_vec)
def __init__(self, normal_vec, d_origin, ref_point, radius):
"""Plane constructor
Args:
normal_vec (tuple (float, float, float)): The normal vector of the plane
d_origin (float): The smallest distance of the plane from the origin
ref_point (tuple (float, float, float)): The center point
for the plane point creation radius
radius (float): The plane point creation radius
"""
n_vec = check_direction_vector(normal_vec)
self.normal_vec = normalize_vector(n_vec)
self.d_origin = check_geometry_parameter(d_origin)
self.radius = check_radius(radius)
self.ref_point = check_vector(ref_point)
if not math.isclose(calc_dot_product(self.normal_vec, self.ref_point) -
self.d_origin,
0.0,
abs_tol=0.000001):
raise ValueError(
"""Invalid reference point. Expected the reference point
to lie on the plane""")