distances

DistanceEngine

DistanceEngine()

DistanceEngine distance computation between a point and a curve in 3D space.

It uses a defined plane for the distance calculation. The plane is defined by a span frame, which is determined by two points (start and end of the span). The engine allows to add curves and span frames, and then compute the distance from a given point to the curve along the plane defined by the span frame. The result is returned as a DistanceResult object containing the distance information and projection details.

Examples:

>>> de = DistanceEngine()
>>> de.add_curves(curve_points)
>>> de.add_span_frame(span_start, span_end)
>>> distance_result = de.plane_distance(obstacle_point)

Source code in src/mechaphlowers/core/geometry/distances.py

def __init__(self):
    pass

axis_points property

axis_points: ndarray

Return the axis points as a numpy array of shape (2, 3) containing the start and end points of the span frame.

add_curves

add_curves(curve_points: ndarray)

add curves to the engine, which will be used for distance calculations. The curves are defined by their points in 3D space.

Parameters:

Name	Type	Description	Default
curve_points	ndarray	A numpy array of shape (N, 3) representing the points of one or more curves in 3D space, concatenated along the first dimension.	required

Source code in src/mechaphlowers/core/geometry/distances.py

def add_curves(self, curve_points: np.ndarray):
    """add curves to the engine, which will be used for distance calculations. The curves are defined by their points in 3D space.

    Args:
        curve_points: A numpy array of shape (N, 3) representing the points of one or more curves in 3D space, concatenated along the first dimension.
    """
    self.curve_points = curve_points

add_span_frame

add_span_frame(x_axis_start: ndarray, x_axis_end: ndarray)

Add a span frame to the engine, which will be used for distance calculations.

The span frame is defined by its X axis start and end points in 3D space. This frame will be used to define the plane for distance calculations, with the normal vector of the plane being vertical (Z direction) and the other two basis vectors defined in the XY plane along the span direction and perpendicular to it.

Warning - Z is always oriented upwards, which is important for the distance calculations.

Parameters:

Name	Type	Description	Default
x_axis_start	ndarray	A numpy array of shape (3,) representing the start point of the span frame.	required
x_axis_end	ndarray	A numpy array of shape (3,) representing the end point of the span frame.	required

Source code in src/mechaphlowers/core/geometry/distances.py

def add_span_frame(self, x_axis_start: np.ndarray, x_axis_end: np.ndarray):
    """Add a span frame to the engine, which will be used for distance calculations.

    The span frame is defined by its X axis start and end points in 3D space.
    This frame will be used to define the plane for distance calculations, with the normal vector of the plane being vertical (Z direction) and the other two basis vectors defined in the XY plane along the span direction and perpendicular to it.

    Warning - Z is always oriented upwards, which is important for the distance calculations.

    Args:
        x_axis_start: A numpy array of shape (3,) representing the start point of the span frame.
        x_axis_end: A numpy array of shape (3,) representing the end point of the span frame.
    """
    self.axis_start = x_axis_start.copy()
    self.axis_end = x_axis_end.copy()

    self.axis_start[2] = 0  # Ensure Z=0 for span frame
    self.axis_end[2] = 0

    self.line_direction = self.axis_end - self.axis_start
    self.line_direction[2] = 0  # ensure projection onto XY plane
    self.line_direction_normalized = self.line_direction / np.linalg.norm(
        self.line_direction
    )

define_distance_plane

define_distance_plane(
    point: ndarray,
) -> tuple[ndarray, ndarray]

Define the distance plane based on a given point.

Parameters:

Name	Type	Description	Default
point	ndarray	A numpy array of shape (3,) representing the point from which the distance plane belongs. This point will be used as the origin of the plane, and the plane will be defined with a normal vector vertical to the span frame (Z direction) and two basis vectors in the XY plane.	required

Returns:

Type	Description
tuple[ndarray, ndarray]	A tuple containing the basis vectors of the plane (u_plane, v_plane).

Source code in src/mechaphlowers/core/geometry/distances.py

def define_distance_plane(
    self, point: np.ndarray
) -> tuple[np.ndarray, np.ndarray]:
    """Define the distance plane based on a given point.

    Args:
        point: A numpy array of shape (3,) representing the point from which the distance plane belongs. This point will be used as the origin of the plane, and the plane will be defined with a normal vector vertical to the span frame (Z direction) and two basis vectors in the XY plane.

    Returns:
        A tuple containing the basis vectors of the plane (u_plane, v_plane).
    """
    # Define plane normal (vertical)
    self.u_plane, self.v_plane, _ = plane_from_line(
        point=point, plane_normal=self.line_direction_normalized
    )
    return self.u_plane, self.v_plane

DistanceResult

DistanceResult(
    point_base: ndarray,
    point_target: ndarray,
    u_plane: ndarray,
    v_plane: ndarray,
    distance_3d: float,
    distance_projection_u: float,
    distance_projection_v: float,
)

Source code in src/mechaphlowers/core/geometry/distances.py

def __init__(
    self,
    point_base: np.ndarray,
    point_target: np.ndarray,
    u_plane: np.ndarray,
    v_plane: np.ndarray,
    distance_3d: float,
    distance_projection_u: float,
    distance_projection_v: float,
):
    self.point_base = point_base
    self.point_target = point_target
    self.distance_3d = distance_3d
    self.signed_distance_projection_u = distance_projection_u
    self.signed_distance_projection_v = distance_projection_v
    self.u_plane = u_plane
    self.v_plane = v_plane

distance_projection_u property

distance_projection_u: float

Return the signed distance projection along the u_plane basis vector.

distance_projection_v property

distance_projection_v: float

Return the signed distance projection along the v_plane basis vector.

get_projection_points

get_projection_points(
    origin_point: ndarray,
    projection_u: float,
    projection_v: float,
    u_plane: ndarray,
    v_plane: ndarray,
) -> tuple[ndarray, ndarray]

Calculate the projection points on the plane basis vectors from an origin point and the projections.

Parameters:

Name	Type	Description	Default
origin_point	ndarray	The original point in 3D space from which projections are calculated (numpy array of shape (3,)).	required
projection_u	float	The scalar projection distance along the u_plane basis vector.	required
projection_v	float	The scalar projection distance along the v_plane basis vector.	required
u_plane	ndarray	The first basis vector of the plane (numpy array of shape (3,)).	required
v_plane	ndarray	The second basis vector of the plane (numpy array of shape (3,))	required

Returns:

Type	Description
tuple[ndarray, ndarray]	A tuple containing the projection points on the plane basis vectors (u_projection_point, v_projection_point).

Source code in src/mechaphlowers/core/geometry/distances.py

def get_projection_points(
    origin_point: np.ndarray,
    projection_u: float,
    projection_v: float,
    u_plane: np.ndarray,
    v_plane: np.ndarray,
) -> tuple[np.ndarray, np.ndarray]:
    """Calculate the projection points on the plane basis vectors from an origin point and the projections.

    Args:
        origin_point: The original point in 3D space from which projections are calculated (numpy array of shape (3,)).
        projection_u: The scalar projection distance along the u_plane basis vector.
        projection_v: The scalar projection distance along the v_plane basis vector.
        u_plane: The first basis vector of the plane (numpy array of shape (3,)).
        v_plane: The second basis vector of the plane (numpy array of shape (3,))

    Returns:
        A tuple containing the projection points on the plane basis vectors (u_projection_point, v_projection_point).
    """
    u_projection_point = origin_point + projection_u * u_plane
    v_projection_point = origin_point + projection_v * v_plane
    return u_projection_point, v_projection_point

points_distance_inside_plane

points_distance_inside_plane(
    point_base: ndarray,
    point_target: ndarray,
    u_plane: ndarray,
    v_plane: ndarray,
) -> tuple[float, float, float]

Compute the distance between two points in 3D inside a plane.

The plane is defined by its basis vectors u_plane and v_plane, which are orthogonal and normalized. The function compute 3D distance between the two points, as well as the projections of this distance onto the plane basis vectors.

Parameters:

Name	Type	Description	Default
point_base	ndarray	The first point in 3D space (numpy array of shape (3,)).	required
point_target	ndarray	The second point in 3D space (numpy array of shape (3,)).	required
u_plane	ndarray	The first basis vector of the plane (numpy array of shape (3,)).	required
v_plane	ndarray	The second basis vector of the plane (numpy array of shape (3,)).	required

Returns:

Type	Description
tuple[float, float, float]	A tuple containing the 3D distance and the projections onto the plane basis vectors (distance_3d, distance_projection_u, distance_projection_v).

Source code in src/mechaphlowers/core/geometry/distances.py

def points_distance_inside_plane(
    point_base: np.ndarray,
    point_target: np.ndarray,
    u_plane: np.ndarray,
    v_plane: np.ndarray,
) -> tuple[float, float, float]:
    """Compute the distance between two points in 3D inside a plane.

    The plane is defined by its basis vectors u_plane and v_plane, which are orthogonal and normalized.
    The function compute 3D distance between the two points, as well as the projections of this distance onto the plane basis vectors.

    Args:
        point_base: The first point in 3D space (numpy array of shape (3,)).
        point_target: The second point in 3D space (numpy array of shape (3,)).
        u_plane: The first basis vector of the plane (numpy array of shape (3,)).
        v_plane: The second basis vector of the plane (numpy array of shape (3,)).

    Returns:
        A tuple containing the 3D distance and the projections onto the plane basis vectors (distance_3d, distance_projection_u, distance_projection_v).
    """

    # Calculate difference vector
    diff_vector = point_target - point_base

    # Calculate 3D distance
    distance_3d = np.linalg.norm(diff_vector)

    # Project onto plane frame basis vectors
    # u_plane: first basis vector in the plane (y-direction in plane frame)
    # v_plane: second basis vector in the plane (z-direction in plane frame)
    distance_projection_u = np.dot(diff_vector, u_plane)
    distance_projection_v = np.dot(diff_vector, v_plane)

    # Verify: the projections should satisfy Pythagorean theorem
    projected_distance = np.sqrt(
        distance_projection_u**2 + distance_projection_v**2
    )
    if abs(projected_distance - distance_3d) > 1e-6:
        raise ValueError(
            f"Projected distance ({projected_distance:.6f} m) does not match 3D distance ({distance_3d:.6f} m) - check calculations!"
        )

    return distance_3d, distance_projection_u, distance_projection_v

planes

change_local_frame

change_local_frame(
    local_frame_origin: ndarray,
    local_frame_x_axis: ndarray,
    local_point: ndarray,
) -> ndarray

change_local_frame transforms local coordinates defined by an origin and a span direction into absolute coordinates in the global frame.

The local frame is defined such that:

• The origin is the reference point in the global frame.
• The x-axis is aligned with the span direction projected onto the XY plane.
• The y-axis is perpendicular to the x-axis in the XY plane counterclockwise.
• The z-axis is vertical (same as global Z).

Parameters:

Name	Type	Description	Default
local_frame_origin	ndarray	Starting point of the span in global coordinates (3D).	required
local_frame_x_axis	ndarray	Ending point of the span in global coordinates (3D).	required
local_point	ndarray	Local coordinates of the point to transform (3D).	required

Returns: Absolute coordinates of the point in the global frame (3D).

Source code in src/mechaphlowers/core/geometry/planes.py

def change_local_frame(
    local_frame_origin: np.ndarray,
    local_frame_x_axis: np.ndarray,
    local_point: np.ndarray,
) -> np.ndarray:
    """change_local_frame transforms local coordinates defined by an origin and a span direction into absolute coordinates in the global frame.

    The local frame is defined such that:
    <ul>
        <li> The origin is the reference point in the global frame. </li>
        <li> The x-axis is aligned with the span direction projected onto the XY plane. </li>
        <li> The y-axis is perpendicular to the x-axis in the XY plane counterclockwise. </li>
        <li> The z-axis is vertical (same as global Z). </li>
    </ul>

    Args:
        local_frame_origin: Starting point of the span in global coordinates (3D).
        local_frame_x_axis: Ending point of the span in global coordinates (3D).
        local_point: Local coordinates of the point to transform (3D).
    Returns:
        Absolute coordinates of the point in the global frame (3D).
    """
    local_point = np.asarray(local_point)
    if local_point.shape != (3,):
        raise ValueError("local_point must be a 1D array of shape (3,)")

    local_frame_origin = np.asarray(local_frame_origin)
    local_frame_x_axis = np.asarray(local_frame_x_axis)

    # Compute span direction in XY plane
    delta_xy = local_frame_x_axis[:2] - local_frame_origin[:2]
    delta_norm = np.linalg.norm(delta_xy)

    if delta_norm == 0:
        raise ValueError("Span direction is zero in XY plane")

    # Construct orthonormal basis for the local frame
    # axis_x: unit vector along span in XY plane
    axis_x = delta_xy / delta_norm
    # axis_y: perpendicular to axis_x in XY plane (rotated 90° counterclockwise)
    axis_y = np.array([-axis_x[1], axis_x[0]])

    # Transform: absolute = origin + x_local * axis_x + y_local * axis_y + z_local * axis_z
    abs_xy = (
        local_frame_origin[:2]
        + local_point[0] * axis_x
        + local_point[1] * axis_y
    )
    abs_z = local_frame_origin[2] + local_point[2]

    return np.array([abs_xy[0], abs_xy[1], abs_z])

compute_plane_normal

compute_plane_normal(key_points: ndarray) -> ndarray

Compute plane normal from two key points.

Parameters:

Name	Type	Description	Default
key_points	ndarray	Array of shape (2, 3) defining start and end of line.	required

Returns:

Type	Description
ndarray	Direction vector from first to second point.

Source code in src/mechaphlowers/core/geometry/planes.py

def compute_plane_normal(key_points: np.ndarray) -> np.ndarray:
    """Compute plane normal from two key points.

    Args:
        key_points: Array of shape (2, 3) defining start and end of line.

    Returns:
        Direction vector from first to second point.
    """
    return key_points[1] - key_points[0]

line_function_from_2_points

line_function_from_2_points(
    p1: ndarray, p2: ndarray
) -> Callable[[float], ndarray]

Returns a function that represents the line defined by two points p1 and p2 in 3D space.

The returned function takes a scalar parameter t and returns the point on the line corresponding to that parameter.

Source code in src/mechaphlowers/core/geometry/planes.py

def line_function_from_2_points(
    p1: np.ndarray, p2: np.ndarray
) -> Callable[[float], np.ndarray]:
    """Returns a function that represents the line defined by two points p1 and p2 in 3D space.

    The returned function takes a scalar parameter t and returns the point on the line corresponding to that parameter.
    """
    p1, line_direction, _ = parametric_line_from_2_points(p1, p2)

    def line_function(t: float) -> np.ndarray:
        return p1 + t * line_direction

    return line_function

parametric_line_from_2_points

parametric_line_from_2_points(
    p1: ndarray, p2: ndarray
) -> tuple[ndarray, ndarray, ndarray]

Returns the coefficients of the line defined by two points p1 and p2 in 3D space.

The line is represented in parametric form as: L(t) = p1 + t * line_direction_normalized

where t is a scalar parameter and line_direction_normalized is the normalized direction vector of the line.

Source code in src/mechaphlowers/core/geometry/planes.py

def parametric_line_from_2_points(
    p1: np.ndarray, p2: np.ndarray
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """Returns the coefficients of the line defined by two points p1 and p2 in 3D space.

    The line is represented in parametric form as:
    L(t) = p1 + t * line_direction_normalized

    where t is a scalar parameter and line_direction_normalized is the normalized direction vector of the line.
    """
    # Direction vector of the line between p1 and p2
    line_direction = p2 - p1
    line_direction_normalized = line_direction / np.linalg.norm(line_direction)

    return p1, line_direction, line_direction_normalized