# Copyright (c) 2025, Maxime Paschoud.
# All rights reserved.
#
# This source code is licensed under the BSD-style license found in the
# LICENSE file in the root directory of this source tree.
#
# (http://opensource.org/licenses/BSD-3-Clause)
#
# __author__ = "Maxime Paschoud, ETHZ: CMBM"
#
from math import cos, sin
import numpy as np
[docs]
def get_rotation_matrix(axis, angle, rad=False):
"""
Returns a rotation matrix for given axis and angle.
Parameters:
- axis: A 3-element array-like specifying the axis of rotation (e.g., [1., 1., 1.])
- angle_degrees: The angle of rotation in degrees
Returns:
- A 3x3 rotation matrix
"""
# Normalize the axis vector
axis = np.asarray(axis)
axis = axis / np.linalg.norm(axis)
# Convert deg to rad
if not rad:
angle = np.deg2rad(angle)
x, y, z = axis
cos_theta = np.cos(angle)
sin_theta = np.sin(angle)
one_minus_cos = 1 - cos_theta
# General 3D rotation matrix
r = np.array([
[cos_theta + x*x*one_minus_cos, x*y*one_minus_cos - z*sin_theta, x*z*one_minus_cos + y*sin_theta],
[y*x*one_minus_cos + z*sin_theta, cos_theta + y*y*one_minus_cos, y*z*one_minus_cos - x*sin_theta],
[z*x*one_minus_cos - y*sin_theta, z*y*one_minus_cos + x*sin_theta, cos_theta + z*z*one_minus_cos]
])
return r
[docs]
def sample_points_on_plane(n, d, size=500, num_points=1000):
"""
Samples random points on a plane defined by the normal vector n and intercept d.
Parameters:
- n: A 3-element array representing the normalized normal vector of the plane.
- d: The intercept of the plane (distance from origin along the normal).
- num_points: The number of random points to sample (default: 1000)
Returns:
- A (num_points, 3) array where each row is a point on the plane.
"""
# Ensure the normal vector is a numpy array
n = np.asarray(n)
# Handle special cases where some components of n might be zero
if np.isclose(n[0], 0) and np.isclose(n[1], 0):
# n is along the z-axis, so x and y can vary freely
basis1 = np.array([1, 0, 0]) # Arbitrary basis vector in the plane
basis2 = np.array([0, 1, 0]) # Another arbitrary basis vector in the plane
elif np.isclose(n[0], 0) and np.isclose(n[2], 0):
basis1 = np.array([1, 0, 0])
basis2 = np.array([0, 0, 1])
elif np.isclose(n[1], 0) and np.isclose(n[2], 0):
basis1 = np.array([0, 1, 0])
basis2 = np.array([0, 0, 1])
else:
# Find two vectors orthogonal to the normal vector n
basis1 = np.cross(n, [1, 0, 0]) if not np.isclose(n[0], 0) else np.cross(n, [0, 1, 0])
basis1 /= np.linalg.norm(basis1) # Normalize the basis vector
basis2 = np.cross(n, basis1)
basis2 /= np.linalg.norm(basis2) # Normalize the second basis vector
# Randomly sample coefficients for the two basis vectors
coeff1 = np.random.uniform(-size, size, num_points)
coeff2 = np.random.uniform(-size, size, num_points)
# Compute the points on the plane
points = np.array([coeff1[i] * basis1 + coeff2[i] * basis2 for i in range(num_points)])
# Calculate the constant term for the plane equation: n . x = -d
# Shift the points to satisfy the intercept (place them on the plane)
shift = (-d - np.dot(points, n))[:, np.newaxis] * n
points += shift
return points