robokudo.utils.math_helper

Functions

intersection_point(P1, P2, t)

Calculate the intersection point based on t value.

distance(point1, point2)

Calculate the Euclidean distance between two points.

intersecting_spheres(P1, P2, spheres)

Check where the first intersection between the line segment described by P1 and P2

intersecting_cuboids(P1, P2, cuboids)

Check where the first intersection between the line segment described by P1 and P2

does_line_intersect_sphere(P1, P2, C, r)

compute_direction_vector_angle() → float)

Computes the angle of a direction vector in degrees relative to the floor.

Module Contents

robokudo.utils.math_helper.intersection_point(P1, P2, t)

Calculate the intersection point based on t value.

robokudo.utils.math_helper.distance(point1, point2)

Calculate the Euclidean distance between two points.

robokudo.utils.math_helper.intersecting_spheres(P1, P2, spheres)

Check where the first intersection between the line segment described by P1 and P2 and the spheres in our parameters happens.

Spheres must be a list of triples (name, center, radius)

Parameters:

  • P1 – Start point of the line segment (x, y, z).

  • P2 – End point of the line segment (x, y, z).

  • spheres – List of tuples (name, center, radius) where center is a tuple (x, y, z).

Returns:

List of tuples (dist to P1, name, center, radius, first_intersection_from_P1).

robokudo.utils.math_helper.intersecting_cuboids(P1, P2, cuboids)

Check where the first intersection between the line segment described by P1 and P2 and the cuboids in our parameters happens. The intersection problem is solved using the slab method (see https://en.wikipedia.org/wiki/Slab_method).

Cuboids must be a list of tuples (name, position, orientation, size)

Parameters:

  • P1 – Start point of the line segment (x, y, z).

  • P2 – End point of the line segment (x, y, z).

  • cuboids – List of tuples (name, position, orientation, size) where position is a tuple (x, y, z), orientation is a tuple (x, y, z, w) representing a quaternion, and size is a tuple (width, height, depth).

Returns:

List of tuples (dist to P1, name, cuboid_min, cuboid_max, first_intersection_from_P1).

robokudo.utils.math_helper.does_line_intersect_sphere(P1, P2, C, r)
robokudo.utils.math_helper.compute_direction_vector_angle(direction_vector: numpy.ndarray, floor: numpy.ndarray = np.array([0, 1, 0], dtype=float)) → float

Computes the angle of a direction vector in degrees relative to the floor. The angle is computed as the arctangent of the y and x components of the vector.

Parameters:

  • direction_vector – A 3D vector (numpy array) representing the direction.

  • floor – A 3D vector (numpy array) representing the floor normal. Default is (0, 1, 0).

Returns:

Angle in degrees.