"""Utility functions for Euclidean space.
It includes parametrizations for circles/spheres
in three-dimensional space and an implementation
of Catmull–Rom splines.
"""
import numpy as np
from scipy.spatial.transform import Rotation as rotation
import ddg
from ddg import nonexact
##################################
# Parametrization of Simple Shapes
##################################
[docs]def torus_parametrization(phi, theta, R, r):
"""
A simple parametrization of the torus.
Parameters
----------
phi : float
The value of the first parameter.
theta : float
The value of the second parameter.
R : float
Radius of the larger circle.
r : float
Radius of the smaller circle.
Returns
-------
np.ndarray of shape (3, 1)
"""
return np.array(
[
(R + r * np.cos(theta)) * np.cos(phi),
(R + r * np.cos(theta)) * np.sin(phi),
r * np.sin(theta),
]
)
[docs]def mobius_strip_parametrization(u, v, R):
"""
A simple parametrization of the moebius strip.
Parameters
----------
u : float
The value of the first parameter.
v : float
The value of the second parameter.
R : float
Radius of the midcircle of the moebius strip.
Returns
-------
np.ndarray of shape (3, 1)
"""
return np.array(
[
(R + (v / 2) * np.cos(u / 2)) * np.cos(u),
(R + (v / 2) * np.cos(u / 2)) * np.sin(u),
(v / 2) * np.sin(u / 2),
]
)
##############################
# Utilities for quadrilaterals
##############################
[docs]def intersect_diags(v1, v2, v3, v4):
r"""
For a quadrilateral(v1,v2,v3,v4) that lies in a plane,
get the intersection point of the diagonals.
The four points are given as an argument in positive cyclic order. ::
v4-----v3
|\ /|
| x |
|/ \|
v1-----v2
Parameters
----------
v1, v2, v3, v4 : numpy.ndarray of shape (n,)
Four points of quadrilateral vertices in positive cyclic order.
Returns
-------
numpy.ndarray of shape (n,)
Notes
-----
Works for arbitrarily many dimensions as long as it is contained in a plane.
"""
v1, v2, v3, v4 = map(np.array, [v1, v2, v3, v4])
m = np.array([v2 - v1, v3 - v1, v4 - v1])
# assert np.linalg.matrix_rank(m) <= 2, "Points are not contained in a plane"
A = np.transpose(np.array([v3 - v1, v4 - v2]))
t = np.linalg.lstsq(A, v2 - v1, rcond=None)[0][0]
return v1 + t * (v3 - v1)
[docs]def intersect_edges(v1, v2, v3, v4):
r"""
For a quadrilateral (v1,v2,v3,v4) that lies in a plane,
get the intersection of the two opposite edges (v1,v2) and (v3,v4). ::
v4-----v3--
| | \
| | x
| | /
v1-----v2--
Parameters
----------
v1, v2, v3, v4 : numpy.ndarray of shape (n,)
Four points of quadrilateral vertices in positive cyclic order.
Returns
-------
numpy.ndarray of shape (n,)
Notes
-----
Works for arbitrarily many dimensions as long as it is contained in a plane.
"""
v1, v2, v3, v4 = map(np.array, [v1, v2, v3, v4])
m = np.array([v3 - v4, v2 - v1])
assert np.linalg.matrix_rank(m) != 1, "Opposite edges are parallel"
m = np.array([v2 - v1, v3 - v1, v4 - v1])
assert np.linalg.matrix_rank(m) <= 2, "Points are not contained in a plane"
A = np.transpose(np.array([v2 - v1, v3 - v4]))
t = np.linalg.lstsq(A, v4 - v1, rcond=None)[0][0]
return v1 + t * (v2 - v1)
[docs]def christoffel_dual_quad(v1, v2, v3, v4):
"""
Compute the Christoffel dual quadrilateral of a given quadrilateral.
Corresponding edges and non-corresponding diagonals are parallel.
This determines the dual quadrilateral up to scaling.
Parameters
----------
v1, v2, v3, v4 : numpy.ndarray of shape (n,)
Coordinates of the given quadrilateral.
Returns
-------
V1, V2, V3, V4 : numpy.ndarray of shape (n,)
Coordinates of the dual quadrilateral.
"""
v1, v2, v3, v4 = map(np.array, [v1, v2, v3, v4])
m = intersect_diags(v1, v2, v3, v4)
alpha = np.linalg.norm(v1 - m)
beta = np.linalg.norm(v2 - m)
gamma = np.linalg.norm(v3 - m)
delta = np.linalg.norm(v4 - m)
if np.dot(v3 - m, v1 - m) < 0:
alpha = -alpha
if np.dot(v2 - m, v4 - m) < 0:
delta = -delta
V1 = np.zeros(v1.size)
V2 = (v2 - v1) / (alpha * beta)
V3 = V2 + (v3 - v2) / (beta * gamma)
V4 = (v4 - v1) / (alpha * delta)
return (V1, V2, V3, V4)
[docs]def focal_points(v1, v2, v3, v4):
r"""
For a quadrilateral (v1, v2, v3, v4),
get the focal points of the quadrilateral.
These are the intersecting points of all edges. ::
p2
/ \
/ \
v4-----v3--
| | \
| | p1
| | /
v1-----v2--
Parameters
----------
v1, v2, v3, v4 : numpy.ndarray of shape (n,)
Coordinates of the given quadrilateral.
Returns
-------
p1, p2 : numpy.ndarray of shape (n,)
Focal points of the given quadrilateral.
"""
v1, v2, v3, v4 = map(np.array, [v1, v2, v3, v4])
p1 = intersect_edges(v1, v2, v3, v4)
p2 = intersect_edges(v2, v3, v4, v1)
return (p1, p2)
[docs]def diagonal_triangle(v1, v2, v3, v4):
r"""
For a quadrilateral (v1, v2, v3, v4),
get the diagonal triangle of the quadrilateral.
These points determine the triangle formed by the
intersection of all diagonals. ::
p3
/ \
/ \
v4-----v3--
|\ /| \
| p1 | p2
|/ \| /
v1-----v2--
Parameters
----------
v1, v2, v3, v4 : numpy.ndarray of shape (n,)
Coordinates of the given quadrilateral.
Returns
-------
p1, p2, p3 : numpy.ndarray of shape (n,)
Diagonal triangle of the given quadrilateral.
"""
v1, v2, v3, v4 = map(np.array, [v1, v2, v3, v4])
p1 = intersect_diags(v1, v2, v3, v4)
p2, p3 = focal_points(v1, v2, v3, v4)
return (p1, p2, p3)
##########################################################
# Utilities for spheres/circles and their parametrizations
##########################################################
[docs]def circle_fct(t, center, radius=1, normals=None):
"""
Parametrization map of a circle with a given center, radius and list of
normals. The dimensions of the normals must match the dimension of the
center and both must be >= 2.
Parameters
----------
t: float
Parameter for the parametrization.
center: numpy.ndarray of shape (d,)
Center of the circle.
radius: float (default=1)
Radius of the circle.
normals: numpy.ndarray of shape (n,d) (default=None)
Normals of the circle that determine the orthogonal complement of the
subspace containing the circle. If None are given the orthogonal
complement will be determined by the dimension of the center and its
normals consist of unit normal vectors.
These vectors must be linearly independent(!)
Returns
-------
float
Value of a point on the circle at parameter t.
Raises
------
ValueError
If the shapes of center and normals do not match.
If the number of normals is not smaller than the dimension
of the circle center minus 2.
"""
center = np.array(center)
dim = center.shape[0]
if normals is None:
normals = [ddg.math.linalg.e(i, dim) for i in range(2, dim)]
elif np.ndim(normals) == 1:
normals = np.array([normals])
if not len(normals) < dim - 1:
raise ValueError(
"The number of normals must be smaller than the dimension of the "
"circle center minus 2. The given circle center has dimension "
f"{dim} but {len(normals)} normals were given."
)
v = np.concatenate((np.zeros(dim - 2), [np.cos(t), np.sin(t)]))
v *= radius
if dim > 2:
if np.shape(normals)[1] != dim:
raise ValueError(
"Shapes of center and normal mismatch. The center has "
f"dimension {dim} and the normals have shape "
f"{np.shape(normals)}, which should be (*, {dim})."
)
rot = extend_to_onb(normals, index=0)
v = rot @ v
return v + center
[docs]def sphere_fct(u, v, center=None, radius=1):
"""
Parameterization map of a 2d sphere in 3d euclidean space
with a given center and radius
Parameters
----------
u: float
First parameter for the parametrization.
v: float
Second parameter for the parametrization.
center: array_like of shape (3,), (default=None)
Center of the sphere.
radius: float (default=1)
Radius of the sphere.
Returns
-------
float
Value of the sphere parametrization for given
parameters u and v.
Raises
------
ValueError
If center is not 3-dimensional.
"""
center = np.array([0, 0, 0]) if center is None else np.array(center)
if len(center) != 3:
raise ValueError(
"The center for the sphere must be 3 dimensional. A"
f" {len(center)} dimensional ndarray was given: {center}."
)
x1 = radius * np.cos(u) * np.sin(v) + center[0]
x2 = radius * np.sin(u) * np.sin(v) + center[1]
x3 = radius * np.cos(v) + center[2]
return np.array([x1, x2, x3])
[docs]def circumcenter(v0, v1, v2):
"""
Computes the center of the circle through the three given points
in two-dimensional or three-dimensional space.
Parameters
----------
v0, v1, v2 : array_like of shape (2,) or (3,)
Points in R^2 or R^3
Returns
-------
numpy.ndarray of shape (2,) or (3,)
Center of the circle through the points v0, v1, v2
"""
v0, v1, v2 = np.array(v0), np.array(v1), np.array(v2)
ec1 = v1 - v0
ec2 = v2 - v1
ec3 = v0 - v2
alpha = (
np.linalg.norm(ec2) ** 2
* (-1)
* np.dot(ec1, ec3)
/ (2 * np.linalg.norm(np.cross(ec1, ec2)) ** 2)
)
beta = (
np.linalg.norm(ec3) ** 2
* (-1)
* np.dot(ec1, ec2)
/ (2 * np.linalg.norm(np.cross(ec1, ec2)) ** 2)
)
gamma = (
np.linalg.norm(ec1) ** 2
* (-1)
* np.dot(ec2, ec3)
/ (2 * np.linalg.norm(np.cross(ec1, ec2)) ** 2)
)
ac1 = alpha * v0
bc2 = beta * v1
gc3 = gamma * v2
return ac1 + bc2 + gc3
[docs]def circle_through_three_points(v0, v1, v2, atol=None, rtol=None):
"""
Computes the center, radius and normal of the circle through the
three given points in three-dimensional space.
Parameters
----------
v0, v1, v2 : array_like of shape (2,) or (3,)
Points in R^2 or R^3.
atol, rtol : float (default=None)
If None, the global defaults will be used.
Returns
-------
numpy.ndarray of shape (2,) (3,), float, numpy.ndarray of shape (3,) or float
Center, radius, normal of the circle through the three
given points, respectively.
If points are 2D then center will have shape (2,) and normal will be float.
Raises
------
ValueError
If the three points given lie on a common line
Notes
-----
Since the cross product of difference vectors of the points
is computed, their dimension must be three.
If 2d points are given instead, the third component is assumed
to be zero and the normal is returned as a value (type float),
such that the normal vector is given by [0, 0, normal]
"""
v0, v1, v2 = np.array(v0), np.array(v1), np.array(v2)
n = np.cross((v1 - v0), (v2 - v0))
if not nonexact.allclose(n, 0.0, atol=atol, rtol=rtol):
n = n / np.linalg.norm(n)
else:
raise ValueError("Not a possible circle. The three points given are colinear!")
cc = circumcenter(v0, v1, v2)
return cc, np.linalg.norm(cc - v0), n
[docs]def sphere_through_four_points(p0, p1, p2, p3):
"""
Computes the center and radius of sphere through the
four given points in three-dimensional space.
Parameters
----------
p0, p1, p2, p3 : array_like of shape (3,)
Points in R^3.
Returns
-------
numpy.ndarray of shape (3,), float
Center, radius of the sphere respectively.
"""
points = [np.array(p0), np.array(p1), np.array(p2), np.array(p3)]
A = [embed(p, level=1, index=0) for p in points]
b = [-np.linalg.norm(p) ** 2 for p in points]
x = np.linalg.solve(A, b)
xm = -x[1] / 2
ym = -x[2] / 2
zm = -x[3] / 2
r2 = xm**2 + ym**2 + zm**2 - x[0]
return [xm, ym, zm], np.sqrt(r2)
##########################################
# Utilities for 2D and 3D transformations
##########################################
[docs]def rotation_2d(alpha):
"""
Returns 2x2 rotation matrix ::
cos(alpha) | -sin(alpha)
A = ------------------------
sin(alpha) | cos(alpha)
Parameters
----------
alpha : float
Rotation angle.
Returns
-------
numpy.ndarray of shape (2, 2)
"""
return np.array([[np.cos(alpha), -np.sin(alpha)], [np.sin(alpha), np.cos(alpha)]])
[docs]def rotation_from_to(source, target, homogeneous=True, atol=None, rtol=None):
"""
Returns a 3x3 rotation matrix taking the source vector to the target vector,
or a 4x4 matrix of the type ::
A | 0
T = -----
0 | 1
Parameters
----------
source : array_like of shape (3,)
Source vector.
target : array_like of shape (3,)
Target vector.
homogeneous: bool, (default=True)
Determines whether 4x4 matrix or 3x3 matrix is returned.
atol, rtol : float (default=None)
If None, the global defaults will be used.
Returns
-------
numpy.ndarray of shape either (3, 3) or (4, 4)
Rotation matrix taking source vector to target vector.
Raises
------
ValueError
If at least one of the vectors is the zero vector (0, 0, 0).
Notes
-----
If source and target are parallel the identety matirx is returned.
If source and target are anti-parallel a correspoding 180 degree
rotation is returned.
"""
if nonexact.allclose(source, 0.0, atol=atol, rtol=rtol) or nonexact.allclose(
target, 0.0, atol=atol, rtol=rtol
):
raise ValueError("Source or target is too close to the zero vector.")
norm_source = np.linalg.norm(source)
norm_target = np.linalg.norm(target)
unit_source = source / norm_source
unit_target = target / norm_target
d = np.dot(unit_source, unit_target)
c = np.cross(unit_source, unit_target)
if (
nonexact.allclose(c, 0.0, atol=atol, rtol=rtol) and d < 0
): # source and target are anti-parallel
# choose some orthogonal axis
a = ddg.math.inner_product.get_row_complement(np.array([source]))[0]
# 180 degree rotation around this axis
T = np.identity(3) + 2 * np.linalg.matrix_power(skew_symmetric_matrix(a), 2)
else:
# rotation matrix around c, mapping source to target
T = (
np.identity(3)
+ skew_symmetric_matrix(c)
+ (1 / (1 + d)) * np.linalg.matrix_power(skew_symmetric_matrix(c), 2)
)
if homogeneous:
T = np.insert(T, 3, np.zeros(3), axis=1)
T = np.insert(T, 3, np.array([0, 0, 0, 1]), axis=0)
return T
[docs]def scale_rotation_from_to(source, target, homogeneous=True, atol=None, rtol=None):
"""
Returns a 3x3 scale rotation matrix taking the source vector to the target vector,
or a 4x4 matrix of the type ::
A | 0
T = -----
0 | 1
Parameters
----------
source : array_like of shape (3,)
Source vector.
target : array_like of shape (3,)
Target vector.
homogeneous: bool, (default=True)
Determines whether 4x4 matrix or 3x3 matrix is returned.
atol, rtol : float (default=None)
If None, the global defaults will be used.
Returns
-------
numpy.ndarray of shape either (3, 3) or (4, 4)
Scale rotation matrix taking source vector to target vector.
Raises
------
ValueError
If at least one of the vectors is the zero vector (0, 0, 0).
"""
T = rotation_from_to(source, target, homogeneous=homogeneous, atol=atol, rtol=rtol)
norm_source = np.linalg.norm(source)
norm_target = np.linalg.norm(target)
T = (
scaleXYZ(np.full(3, norm_target), homogeneous=homogeneous)
[docs] @ T
@ scaleXYZ(np.full(3, 1 / norm_source), homogeneous=homogeneous)
)
return T
def rotation_angle_axis(rotation_vector, homogeneous=True):
"""
This function parameterizes rotation using
axis–angle representation.
It returns either 4x4 matrix of the type ::
A | 0
M = -----
0 | 1
where A represents the rotation matrix
or A as a 3x3 rotation matrix.
Parameters
----------
rotation_vector : array_like of shape (3,)
A 3-dimensional vector which is co-directional to the axis
of rotation and whose norm gives the angle of rotation.
homogeneous: bool, (default=True)
Determines whether 4x4 matrix or 3x3 matrix is returned.
Returns
-------
numpy.ndarray of shape (3, 3) or (4, 4)
"""
T = rotation.from_rotvec(rotation_vector).as_matrix()
if homogeneous:
T = np.insert(T, 3, np.zeros(3), axis=1)
T = np.insert(T, 3, np.array([0, 0, 0, 1]), axis=0)
return T
[docs]def scaleXYZ(xyz=(1, 1, 1), homogeneous=True):
"""
Returns either 4x4 matrix of type ::
A | 0
M = -----
0 | 1
where A denotes diagonal scaling matrix,
or A as 3x3 diagonal scaling matrix.
Parameters
----------
xyz : array_like of shape (3,), (default=(1, 1, 1))
Scaling factors in x, y and z directions.
homogeneous: bool, (default=True)
Determines whether 4x4 matrix or 3x3 matrix is returned.
Returns
-------
numpy.ndarray of shape (3, 3) or (4, 4)
Scaling matrix in either affine or homogeneous coordinates.
"""
if homogeneous:
xyz = np.append(np.array(xyz), 1)
return np.diag(np.array(xyz))
[docs]def translation_to(b=(0, 0, 0)):
"""
Returns 4x4 matrix of type ::
I | b
M = -----
0 | 1
where I denotes 3x3 identity matrix and
b denotes 3x1 translation vector.
Parameters
----------
b : array_like of shape (3,), (default=(0, 0, 0))
Translation vector.
Returns
-------
numpy.ndarray of shape (4, 4)
"""
T = np.eye(4)
T[:3, 3] = b
return T
[docs]def reflection_in_a_hyperplane(normal, level):
"""Calculate the reflection matrix (in homogeneous coordinates) of
a hyperplane.
Parameters
----------
normal : array_like of shape (n,)
Normal of the plane
level : float
Level of the plane
Returns
-------
reflection_matrix : np.ndarray of shape (n + 1, n + 1)
Raises
------
ValueError
If the given normal does not have shape (n,).
Examples
--------
>>> import numpy as np
>>> from ddg.math.euclidean import reflection_in_a_hyperplane as re
>>> M = re((1, 0), 0)
>>> M
array([[-1., 0., 0.],
[ 0., 1., 0.],
[ 0., 0., 1.]])
>>> p = (-5, 10, 1)
>>> reflected_p = M.dot(p)
>>> reflected_p
array([ 5., 10., 1.])
>>> M = re((1, 1, 1), 5)
>>> M
array([[ 0.33333333, -0.66666667, -0.66666667, 5.77350269],
[-0.66666667, 0.33333333, -0.66666667, 5.77350269],
[-0.66666667, -0.66666667, 0.33333333, 5.77350269],
[ 0. , 0. , 0. , 1. ]])
>>> p = (0, 0, 0, 1)
>>> reflected_p = M.dot(p)
>>> reflected_p
array([5.77350269, 5.77350269, 5.77350269, 1. ])
"""
n = normalize(normal)
if n.ndim != 1:
raise ValueError("The given normal is not a vector.")
dim = n.shape[0]
sub_matrix = np.eye(dim, dim) - 2 * np.outer(n, n.T)
inner_matrix = np.insert(sub_matrix, dim, 2 * level * n, axis=1)
reflection_matrix = np.insert(
inner_matrix, dim, np.append(np.zeros(dim), 1), axis=0
)
return reflection_matrix
#####################
# Catmull–Rom Spline
#####################
[docs]def catmull_rom_spline(P0, P1, P2, P3, alpha=0.5, nPoints=100):
"""
Computes the points of the Catmull-Rom spline segment,
using four control points.
Parameters
----------
P0 : array_like of shape (2,) or (3,)
First control point, which is used to determine
curvature of spline segment between P1 and P2.
P1 : array_like of shape (2,) or (3,)
Second control point, which forms one end of the
spline segment.
P2 : array_like of shape (2,) or (3,)
Third control point, which forms the other end of
the spline segment.
P3 : array_like of shape (2,) or (3,)
Fourth control point, which is used to determine
curvature of spline segment between P1 and P2.
alpha : float (default=0.5)
Spline Knot parameter, ranges from 0 to 1.
nPoints : int (default=100)
Number of points making up the resulting spline segment.
Returns
-------
numpy.ndarray of shape (nPoints, 2) or (nPoints, 3)
The Catmull-Rom spline segment between points P1 and P2.
Notes
-----
Depending on the value of alpha, we get uniform (alpha=0),
centripetal (alpha=0.5) and chordal (alpha=1) parameterization
of Catmull–Rom spline.
"""
P0, P1, P2, P3 = map(np.array, [P0, P1, P2, P3])
# Calculate t0 to t3
def tj(ti, Pi, Pj):
return (np.linalg.norm(Pi - Pj)) ** alpha + ti
t0 = 0
t1 = tj(t0, P0, P1)
t2 = tj(t1, P1, P2)
t3 = tj(t2, P2, P3)
# Only calculate points between P1 and P2
t = np.linspace(t1, t2, nPoints)
# Reshape so that we can multiply by the points P0 to P3
# and get a point for each value of t.
t = t.reshape(len(t), 1)
A1 = (t1 - t) / (t1 - t0) * P0 + (t - t0) / (t1 - t0) * P1
A2 = (t2 - t) / (t2 - t1) * P1 + (t - t1) / (t2 - t1) * P2
A3 = (t3 - t) / (t3 - t2) * P2 + (t - t2) / (t3 - t2) * P3
B1 = (t2 - t) / (t2 - t0) * A1 + (t - t0) / (t2 - t0) * A2
B2 = (t3 - t) / (t3 - t1) * A2 + (t - t1) / (t3 - t1) * A3
C = (t2 - t) / (t2 - t1) * B1 + (t - t1) / (t2 - t1) * B2
return C
[docs]def catmull_rom_curve(P, alpha=0.5, nPoints=4):
"""
Calculate Catmull Rom for a chain of points and
return the combined curve.
Parameters
----------
P : array_like of array_likes each of shape (2,) or (3,)
Chain of points from which the quadruples for
the catmull_rom_spline function are taken.
alpha : float (default=0.5)
Spline Knot parameter, ranges from 0 to 1.
nPoints : int (default=4)
The number of points to include in each curve segment.
Returns
-------
numpy.ndarray of shape (m, 2) or (m, 3)
Catmull-Rom curve made up of n-1 spline segments, each
containing nPoints, i.e m = 2 - n + nPoints*(n-1) where
n denotes the length of given list of points.
Notes
-----
The resulting curve passes through all the given points.
The centripetal parameterization (alpha=0.5) of Catmull-Rom curve
is the only parameterization that guarantees that the curves do not
form cusps or self-intersections within its segments.
"""
P = np.asanyarray(P)
size = len(P)
C = []
for i in range(size - 1):
if i == 0:
c = catmull_rom_spline(2 * P[0] - P[1], P[0], P[1], P[2], alpha, nPoints)
C.extend(c)
elif i == size - 2:
c = catmull_rom_spline(
P[size - 3],
P[size - 2],
P[size - 1],
2 * P[size - 1] - P[size - 2],
alpha,
nPoints,
)
C.extend(c[1:])
else:
c = catmull_rom_spline(P[i - 1], P[i], P[i + 1], P[i + 2], alpha, nPoints)
C.extend(c[1:])
return np.array(C)
##########################
# Other utility functions
##########################
[docs]def extend_to_onb(vectors, index=0):
"""
Extends a given list of vectors to an orthonormal
basis for n-dimensional space.
Parameters
----------
vectors : list of numpy.ndarray of shape (n,) or a numpy.ndarray of shape (k, n)
List of linearly independent vectors to complete into an
orthonormal basis.
index : int (default=0)
If only one vector is given, it's possible to specify
its index in the resulting orthonormal basis.
If more than one vector is given, the first k columns of the
result will have the same span as the given family of vectors.
Returns
-------
numpy.ndarray of shape (n, n)
Columns of resulting matrix form an orthonormal basis.
Raises
------
ValueError
If vectors are given as numpy.ndarray of dimension
greater than 2.
ValueError
If the given family of vectors is linearly dependent.
Notes
-----
The given vector(s) will be normalized.
This function uses Gram-Schmidt, so it might be a bit unstable.
If the vectors are given as a 2-dimensional numpy.ndarray, the
rows of the matrix will be considered as input.
"""
vectors = np.array(vectors)
if vectors.ndim not in [1, 2]:
raise ValueError(
f"Can not extend a {vectors.ndim}-dimensional "
"numpy array to an orthonormal basis."
)
if vectors.ndim == 1:
vectors = [vectors]
if np.linalg.matrix_rank(vectors) != len(vectors):
raise ValueError("The given family of vectors is linearly dependent.")
n = vectors[0].shape[0]
k = len(vectors)
onb = np.zeros((n, n))
onb.T[0] = vectors[0] / np.linalg.norm(vectors[0])
for i in range(1, k):
v = vectors[i]
u = onb.T[i - 1]
proj = np.dot(u, v) / np.dot(u, u)
tmp = v - proj * u
onb.T[i] = tmp / np.linalg.norm(tmp)
if k != n:
eye = np.eye(n)
for i in range(k, n):
temp = eye - onb @ onb.T
j = np.where(sum(nonexact.isclose(temp, 0)) < n)[0][0]
onb.T[i] = temp.T[j] / np.linalg.norm(temp.T[j])
if len(vectors) == 1 and index != 0:
onb[:, [0, index]] = onb[:, [index, 0]]
return onb
[docs]def distance(p0, p1):
"""
Computes the Euclidean distance between two given
point in n-dimensional space.
Parameters
----------
p0, p1 : array_like of shape (n,)
Points to calculate distance of.
Returns
-------
float
Euclidean distance, i.e l2 norm of difference
between the given two point.
"""
diff = np.subtract(np.array(p0), np.array(p1))
return np.linalg.norm(diff)
[docs]def distance_lines(a, v, b, w):
"""
Calculate the shortest distance between the lines a + sv and b + tw.
This only works in 3D-space.
Parameters
----------
a, b : numpy.ndarray of shape (3,)
Initial vector of line.
v, w : array_like of shape (3,)
Direction vector of line.
Returns
-------
float
Shortest distance of line.
"""
n = np.cross(v, w)
n = n / np.linalg.norm(n)
return np.abs(np.dot(n, b - a))
[docs]def skew_symmetric_matrix(v):
"""
Returns the skew-symmetric matrix ::
0 | -v3 | v2
----+------+----
v3 | 0 | -v1
----+------+----
-v2 | v1 | 0
Parameters
----------
v : numpy.ndarray of shape (3,)
Returns
-------
numpy.ndarray of shape (3, 3)
"""
return np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
[docs]def normalize(v):
"""
Normalizes a given vector in n-dimensional space.
Parameters
----------
v : array_like of shape (n,)
The vector to normalize.
Returns
-------
numpy.ndarray of shape (n,)
The normalized vector if it is non-zero,
the zero vector otherwise.
"""
vector = np.array(v)
if not all(vector == 0):
return vector / np.linalg.norm(vector)
else:
return vector
[docs]def embed(v, level=0, index=-1):
"""Embeds a coordinate vector by inserting the given level.
Parameters
----------
v : array_like of shape (n,)
The vector to be embedded.
level : float (default=0)
The value to insert.
index : int (default=-1)
Index of inserted value in the output.
Returns
-------
numpy.ndarray of shape (n+1,)
"""
i = index
vector = np.array(v)
if -np.size(vector) <= i <= -1:
i += np.size(vector) + 1
return np.insert(vector, i, level)
[docs]def angle_bisector_orientation_preserving(n1, d1, n2, d2):
"""Calculate the orientation preserving angle bisector of two hyperplanes.
The hyperplanes are represented by a n-dimensional normal vector and a level.
Parameters
----------
n1 : array_like of shape (n,)
Normal of first hyperplane.
d1 : float
Level of first hyperplane (distace to origin).
n2 : array_like of shape (n,)
Normal of second hyperplane.
d2 : float
Level of second hyperplane (distace to origin).
Returns
-------
n, d : numpy.ndarray of shape (n,), float
Normal and level of orientation preserving angle bisector.
"""
n1_normed = normalize(n1)
n2_normed = normalize(n2)
n = n1_normed + n2_normed
n_norm = np.linalg.norm(n)
d = d1 + d2
return n / n_norm, d / n_norm
[docs]def angle_bisector_orientation_reversing(n1, d1, n2, d2):
"""Calculate the orientation reversing angle bisector of two hyperplanes.
The hyperplanes are represented by a n-dimensional normal vector and a level.
Parameters
----------
n1 : array_like of shape (n,)
Normal of first hyperplane.
d1 : float
Level of first hyperplane (distace to origin).
n2 : array_like of shape (n,)
Normal of second hyperplane.
d2 : float
Level of second hyperplane (distace to origin).
Returns
-------
n, d : numpy.ndarray of shape (n,), float
Normal and level of orientation reversing angle bisector.
"""
n1_normed = normalize(n1)
n2_normed = normalize(n2)
n = n1_normed - n2_normed
n_norm = np.linalg.norm(n)
d = d1 - d2
return n / n_norm, d / n_norm