Implementation:Facebookresearch Habitat lab Geometry Utils

Knowledge Sources	Facebookresearch_Habitat_lab
Domains	Embodied_AI, Computational_Geometry
Last Updated	2026-02-15 00:00 GMT

Overview

This module provides quaternion operations, coordinate transformations, and triangle geometry utilities used throughout Habitat for spatial reasoning and agent state management.

Description

The module provides a comprehensive set of geometric utility functions organized into three categories:

Quaternion Operations:

• angle_between_quaternions(q1, q2) -- Returns the positive angle (in radians) between two quaternions using the inverse-multiply approach with arctan2 for numerical stability.
• quaternion_from_two_vectors(v0, v1) -- Computes the quaternion that rotates vector v0 to v1. Handles the near-antiparallel case (c < -1 + epsilon) using SVD decomposition.
• quaternion_to_list(q) -- Converts a numpy quaternion to a list in [x, y, z, w] format (imaginary components first, then real).
• quaternion_from_coeff(coeffs) -- Creates a numpy quaternion from coefficients in [x, y, z, w] format.
• quaternion_rotate_vector(quat, v) -- Rotates a 3D vector by a quaternion using the sandwich product q * v * q_inv.

Coordinate Transformations:

• agent_state_target2ref(ref_agent_state, target_agent_state) -- Transforms a target agent state (rotation, position) from global coordinates into the local coordinate system defined by a reference agent state. Returns the relative rotation and position. Supports both quaternion objects and [x, y, z, w] coefficient lists as input.

Triangle Geometry:

• random_triangle_point(v0, v1, v2) -- Samples a uniformly random point from a triangle using the point-picking method from Wolfram MathWorld, with a fold-back technique to map points outside the triangle back inside.
• is_point_in_triangle(p, v0, v1, v2) -- Tests if a point lies within a triangle using the cross-product alignment method: translates the triangle so the point is at the origin, then checks that all sub-triangle cross products are aligned.

A module-level constant EPSILON = 1e-8 is used for numerical stability in quaternion operations.

Usage

Use these functions for agent state transformations in navigation tasks, computing relative positions between agents, quaternion manipulations for sensor orientation, and geometric computations for scene understanding.

Code Reference

Source Location

• Repository: Facebookresearch_Habitat_lab
• File: habitat-lab/habitat/utils/geometry_utils.py
• Lines: 1-168

Signature

def angle_between_quaternions(
    q1: quaternion.quaternion, q2: quaternion.quaternion
) -> float:

def quaternion_from_two_vectors(
    v0: np.ndarray, v1: np.ndarray
) -> quaternion.quaternion:

def quaternion_to_list(q: quaternion.quaternion):

def quaternion_from_coeff(coeffs: List[float]) -> quaternion.quaternion:

def quaternion_rotate_vector(
    quat: quaternion.quaternion, v: np.ndarray
) -> np.ndarray:

def agent_state_target2ref(
    ref_agent_state: Union[List, Tuple],
    target_agent_state: Union[List, Tuple]
) -> Tuple[quaternion.quaternion, np.ndarray]:

def random_triangle_point(
    v0: np.ndarray, v1: np.ndarray, v2: np.ndarray
) -> np.ndarray:

def is_point_in_triangle(
    p: np.ndarray, v0: np.ndarray, v1: np.ndarray, v2: np.ndarray
) -> bool:

Import

from habitat.utils.geometry_utils import (
    angle_between_quaternions,
    quaternion_from_two_vectors,
    quaternion_to_list,
    quaternion_from_coeff,
    quaternion_rotate_vector,
    agent_state_target2ref,
    random_triangle_point,
    is_point_in_triangle,
)

I/O Contract

Inputs (angle_between_quaternions)

Name	Type	Required	Description
q1	quaternion.quaternion	Yes	First quaternion
q2	quaternion.quaternion	Yes	Second quaternion

Inputs (quaternion_from_two_vectors)

Name	Type	Required	Description
v0	np.ndarray	Yes	Origin vector (will be normalized)
v1	np.ndarray	Yes	Target vector (will be normalized)

Inputs (agent_state_target2ref)

Name	Type	Required	Description
ref_agent_state	Union[List, Tuple]	Yes	Reference agent state as [rotation, position] in global coordinates
target_agent_state	Union[List, Tuple]	Yes	Target agent state as [rotation, position] in global coordinates

Inputs (is_point_in_triangle)

Name	Type	Required	Description
p	np.ndarray	Yes	The point to test
v0	np.ndarray	Yes	First vertex of the triangle
v1	np.ndarray	Yes	Second vertex of the triangle
v2	np.ndarray	Yes	Third vertex of the triangle

Outputs

Name	Type	Description
angle_between_quaternions()	float	Positive angle in radians between the two quaternions
quaternion_from_two_vectors()	quaternion.quaternion	Quaternion rotating v0 to v1
quaternion_to_list()	List[float]	Quaternion as [x, y, z, w] list
quaternion_from_coeff()	quaternion.quaternion	Numpy quaternion from [x, y, z, w] coefficients
quaternion_rotate_vector()	np.ndarray	The rotated 3D vector
agent_state_target2ref()	Tuple[quaternion.quaternion, np.ndarray]	(relative_rotation, relative_position) in reference coordinate frame
random_triangle_point()	np.ndarray	Random point uniformly sampled from the triangle
is_point_in_triangle()	bool	True if the point lies within the triangle

Usage Examples

Basic Usage

import numpy as np
import quaternion
from habitat.utils.geometry_utils import (
    angle_between_quaternions,
    quaternion_from_two_vectors,
    quaternion_to_list,
    quaternion_from_coeff,
    quaternion_rotate_vector,
    agent_state_target2ref,
    random_triangle_point,
    is_point_in_triangle,
)

# Compute angle between two rotations
q1 = quaternion.quaternion(1, 0, 0, 0)  # identity
q2 = quaternion.quaternion(0.707, 0, 0.707, 0)  # 90 degrees around Y
angle = angle_between_quaternions(q1, q2)

# Find the rotation from one direction to another
v0 = np.array([1.0, 0.0, 0.0])
v1 = np.array([0.0, 1.0, 0.0])
q = quaternion_from_two_vectors(v0, v1)

# Convert quaternion to/from list format
q_list = quaternion_to_list(q)  # [x, y, z, w]
q_back = quaternion_from_coeff(q_list)

# Rotate a vector by a quaternion
rotated = quaternion_rotate_vector(q, np.array([1.0, 0.0, 0.0]))

# Transform target state to reference frame
ref_state = ([0, 0, 0, 1], [0.0, 0.0, 0.0])
target_state = ([0, 0, 0, 1], [1.0, 0.0, 1.0])
rel_rot, rel_pos = agent_state_target2ref(ref_state, target_state)

# Sample a random point from a triangle
v0 = np.array([0.0, 0.0, 0.0])
v1 = np.array([1.0, 0.0, 0.0])
v2 = np.array([0.0, 1.0, 0.0])
point = random_triangle_point(v0, v1, v2)
inside = is_point_in_triangle(point, v0, v1, v2)  # True

Related Pages

• Facebookresearch_Habitat_lab_Task_Utils
• Facebookresearch_Habitat_lab_Core_Utils