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""")