robokudo.utils.math_helper¶

3D geometry math utilities for RoboKudo.

This module provides utilities for 3D geometric calculations.

module:: math_helper
synopsis:: 3D geometric calculations and utilities
moduleauthor:: RoboKudo Team
Dependencies:: None

Functions¶

`intersection_point`(→ Tuple[float, float, float])	Calculate point along line segment using parameter t.
`distance`(→ float)	Calculate Euclidean distance between two 3D points.
`intersecting_spheres`(→ List[Tuple[float, str, ...)	Check where the first intersection between the line segment described by P1 and P2
`compute_line_intersection_point`(→ Optional[List[float]])	Compute the intersection points between a sphere and a line segment.
`does_line_intersect_sphere`(→ bool)	Test if line segment intersects sphere.
`compute_direction_vector_angle`() → float)	Computes the angle of a direction vector in degrees relative to the floor.
`intersecting_cuboids`(→ List[Tuple[float, str, ...)	Check where the first intersection between the line segment described by P1 and P2

Module Contents¶

robokudo.utils.math_helper.intersection_point(point1: Tuple[float, float, float], point2: Tuple[float, float, float], t: float) → Tuple[float, float, float]¶

Calculate point along line segment using parameter t.

Parameters:

point1 – Start point (x,y,z)
point2 – End point (x,y,z)
t – Interpolation parameter [0,1]

Returns:

Interpolated point (x,y,z)

Example:

P1 = (0, 0, 0)
P2 = (1, 1, 1)
mid = intersection_point(P1, P2, 0.5)  # Returns (0.5, 0.5, 0.5)

Note

Uses linear interpolation: P = P1 + t*(P2 - P1)

robokudo.utils.math_helper.distance(point1: Tuple[float, float, float], point2: Tuple[float, float, float]) → float¶

Calculate Euclidean distance between two 3D points.

Parameters:

point1 – First point (x,y,z)
point2 – Second point (x,y,z)

Returns:

Euclidean distance

Example:

p1 = (0, 0, 0)
p2 = (1, 1, 1)
d = distance(p1, p2)  # Returns sqrt(3)

robokudo.utils.math_helper.intersecting_spheres(point1: Tuple[float, float, float], point2: Tuple[float, float, float], spheres: Iterable[Tuple[str, Tuple[float, float, float], float]]) → List[Tuple[float, str, Tuple[float, float, float], float, Tuple[float, float, float]]]¶

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:

point1 – Start point of the line segment (x, y, z).
point2 – End point of the line segment (x, y, z).
spheres – Iterable of tuples (name, center, radius) where center is a tuple (x, y, z).

Returns:

List of tuples (dist to point1, name, center, radius, first_intersection_from_point1).

Example:

P1 = (0, 0, 0)
P2 = (1, 1, 1)
spheres = [
    ("sphere1", (0.5, 0.5, 0.5), 0.1),
    ("sphere2", (0.7, 0.7, 0.7), 0.1)
]
intersections = intersecting_spheres(P1, P2, spheres)

Note

Returns intersections sorted by distance from point1

robokudo.utils.math_helper.compute_line_intersection_point(point1: Tuple[float, float, float], point2: Tuple[float, float, float], sphere_center: Tuple[float, float, float], sphere_radius: float) → List[float] | None¶

Compute the intersection points between a sphere and a line segment.

Parameters:

point1 – First point of the line (x, y, z)
point2 – Second point of the line (x, y, z)
sphere_center – Center of the sphere
sphere_radius – Radius of the sphere

Returns:

List of intersection points on the line or None if line does not intersect

robokudo.utils.math_helper.does_line_intersect_sphere(point1: Tuple[float, float, float], point2: Tuple[float, float, float], sphere_center: Tuple[float, float, float], sphere_radius: float) → bool¶

Test if line segment intersects sphere.

Parameters:

point1 – Line segment start point (x,y,z)
point2 – Line segment end point (x,y,z)
sphere_center – Sphere center point (x,y,z)
sphere_radius – Sphere radius

Returns:

True if line segment intersects sphere

Example:

point1 = (0, 0, 0)
point2 = (1, 1, 1)
sphere_center = (0.5, 0.5, 0.5)
sphere_radius = 0.1
intersects = does_line_intersect_sphere(point1, point2, sphere_center, sphere_radius)

Note

Uses quadratic equation to find intersection points

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. :param direction_vector: A 3D vector (numpy array) representing the direction. :param floor: A 3D vector (numpy array) representing the floor normal. Default is (0, 1, 0). :return: Angle in degrees.

robokudo.utils.math_helper.intersecting_cuboids(point1: Tuple[float, float, float], point2: Tuple[float, float, float], cuboids: List[Tuple[str, Tuple[float, float, float], Tuple[float, float, float, float], Tuple[float, float, float]]]) → List[Tuple[float, str, numpy.ndarray, numpy.ndarray, numpy.ndarray]]¶

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:

point1 – Start point of the line segment (x, y, z).
point2 – 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).