"""Utility functions for Euclidean space.
It includes parametrizations for circles/spheres
in three-dimensional space and an implementation
of Catmull–Rom splines.
"""
import ddg
import numpy as np
##########################################################
# 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.
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(!)
radius: float (default=1)
Radius of the circle.
Returns
-------
float
Value of a point on the circle at parameter t.
"""
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(
f'The number of normals must be smaller than the dimension of the '
f'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(
f'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.
radius: float (default=1)
Radius of the sphere.
center: numpy.ndarray of shape (3,) (default=np.array([0, 0, 0]))
Center of the sphere.
Returns
-------
float
Value of the sphere parametrization for given
parameters u and v.
"""
center = np.array([0, 0, 0]) if center is None else center
if len(center) != 3:
raise ValueError(f'The center for the sphere must be 3 dimensional. A {len(center)} dimensional ndarray was '
f'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 : tuple of float or numpy.ndarray of shape either (2,) or (3,)
First vertex
v1 : tuple of float or numpy.ndarray of shape either (2,) or (3,)
Second vertex
v2 : tuple of float or numpy.ndarray of shape either (2,) or (3,)
Third vertex
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):
"""
Computes the center, radius and normal of the circle through the
three given points in three-dimensional space.
Parameters
----------
v0 : tuple of float or numpy.ndarray of shape either (3,)
First vertex
v1 : tuple of float or numpy.ndarray of shape either (3,)
Second vertex
v2 : tuple of float or numpy.ndarray of shape either (3,)
Third vertex
Returns
-------
numpy.ndarray of shape (3,), float, numpy.ndarray of shape (3,)
Center, radius, normal of the circle through the three
given points, respectively.
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 np.all(n == 0):
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 : tuple of float or numpy.ndarray of shape either (3,)
First point
p1 : tuple of float or numpy.ndarray of shape either (3,)
Second point
p2 : tuple of float or numpy.ndarray of shape either (3,)
Third point
p3 : tuple of float or numpy.ndarray of shape either (3,)
Fourth point
Returns
-------
numpy.ndarray of shape (3,), float
Center, radius of the sphere respectively.
"""
from ddg.math.projective import homogenize
points = [np.array(p0), np.array(p1), np.array(p2), np.array(p3)]
A = [homogenize(p, affine_component=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
##########################################
from scipy.spatial.transform import Rotation as rotation
[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=(0, 0, 1), target=(1, 0, 0), homogenous=True):
"""
Returns either 4x4 matrix of the type
| A | 0
| T = -----
| 0 | 1
where A is 3x3 rotation matrix taking the source vector to the target vector
or A as a 3x3 rotation matrix.
Parameters
----------
source : tuple of float or numpy.ndarray of shape (3,), (default=(0, 0, 1))
Source vector
target : tuple of float or numpy.ndarray of shape (3,), (default=(1, 0, 0))
Target vector
homogenous: bool, (default=True)
Determines whether 4x4 matrix or 3x3 matrix
is returned
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
-----
The rotation axis computed is the cross product of source and target,
so the order of the given vectors matters (!).
"""
if np.linalg.norm(source) == 0 or np.linalg.norm(target) == 0:
raise ValueError(
'The zero vector (0, 0, 0) is not allowed to compute rotation matrix')
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 all(c == 0):
T = np.eye(3)
else:
R = np.identity(3) + skew_symmetric_matrix(c) + (1/(1+d)) * \
np.linalg.matrix_power(skew_symmetric_matrix(c), 2)
T = scaleXYZ(np.full(3, norm_target), homogenous=False) @ R @ \
scaleXYZ(np.full(3, 1/norm_source), homogenous=False)
if homogenous:
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 rotation_angle_axis(rotation_vector, homogenous=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 : tuple of float or numpy.ndarray of shape (3,)
A 3-dimensional vector which is co-directional to the axis
of rotation and whose norm gives the angle of rotation
homogenous: bool, (default=True)
Determines whether 4x4 matrix or 3x3 matrix
is returned
Returns
-------
numpy.ndarray of shape either (3, 3) or (4, 4)
"""
T = rotation.from_rotvec(rotation_vector).as_matrix()
if homogenous:
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), homogenous=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 : tuple of float or numpy.ndarray of shape (3,), (default=(1, 1, 1))
Scaling factors in x, y and z directions
homogenous: bool, (default=True)
Determines whether 4x4 matrix or 3x3 matrix
is returned
Returns
-------
numpy.ndarray of shape either (3, 3) or (4, 4)
Scaling matrix in either affine or homogenous coordinates
"""
if homogenous:
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 : tuple of float or numpy.ndarray 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
#####################
# 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 : numpy.ndarray of shape either (2,) or (3,)
First control point, which is used to determine
curvature of spline segment between P1 and P2
P1 : numpy.ndarray of shape either (2,) or (3,)
Second control point, which forms one end of the
spline segment
P2 : numpy.ndarray of shape either (2,) or (3,)
Third control point, which forms the other end of
the spline segment
P3 : numpy.ndarray of shape either (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 either (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 : list of numpy.ndarray, 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 either (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.
"""
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 each 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 "
f"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(np.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 : tuple of float or numpy.ndarray of shape (n,)
First point
p1 : tuple of float or numpy.ndarray of shape (n,)
Second point
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 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 either (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 : tuple of float or numpy.ndarray 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, affine_component=-1):
"""Embeds a coordinate vector by inserting the
given level.
Parameters
----------
vector : tuple of float or numpy.ndarray of shape (n,)
The vector to be embedded
level : float (default=0)
The value to insert
i : int (default=-1)
Index of inserted value in the output
Returns
-------
numpy.ndarray of shape (n+1,)
"""
i = affine_component
vector = np.array(v)
if -np.size(vector) <= i <= -1:
i += np.size(vector) + 1
return np.insert(vector, i, level)