"""
Routines for working with vectors
These routines can be used with vectors, as well as with matrices containing a vector in each row.
"""
"""
author : Thomas Haslwanter
date : Oct-2020
"""
import numpy as np
from deprecated import deprecated
# The following construct is required since I want to run the module as a script
# inside the skinematics-directory
import os
import sys
file_dir = os.path.dirname(__file__)
if file_dir not in sys.path:
sys.path.insert(0, file_dir)
import quat
# For deprecation warnings
# import deprecation
import warnings
# warnings.simplefilter('always', DeprecationWarning)
[docs]
def normalize(v):
"""Normalization of a given vector (with image)
Parameters
----------
v : array (N,) or (M,N)
input vector
Returns
-------
v_normalized : array (N,) or (M,N)
normalized input vector
.. image:: ../docs/Images/vector_normalize.png
:scale: 33%
Example
-------
>>> skinematics.vector.normalize([3, 0, 0])
array([[ 1., 0., 0.]])
>>> v = [[np.pi, 2, 3], [2, 0, 0]]
>>> skinematics.vector.normalize(v)
array([[ 0.6569322 , 0.41821602, 0.62732404],
[ 1. , 0. , 0. ]])
Notes
-----
.. math::
\\vec{n} = \\frac{\\vec{v}}{|\\vec{v}|}
"""
from numpy.linalg import norm
# Distinguish between a vector and a matrix
if np.array(v).ndim == 1:
vectorFlag = True
else:
vectorFlag = False
# The 'atleast_2d' ensures that the program works on matrices.
# The 'double' avoids trouble 2 lines down, if v is integer.
# And the 'copy' ensures that the input is not modified in the calling program.
v = np.double(np.atleast_2d(v)).copy()
length = norm(v, axis=1)
v[length != 0] = (v[length != 0].T / length[length != 0]).T
if vectorFlag:
v = v.ravel()
return v
[docs]
def angle(v1, v2):
"""Angle between two vectors
Parameters
----------
v1 : array (N,) or (M,N)
vector 1
v2 : array (N,) or (M,N)
vector 2
Returns
-------
angle : double or array(M,)
angle between v1 and v2
.. image:: ../docs/Images/vector_angle.png
:scale: 33%
Example
-------
>>> v1 = np.array([[1,2,3],
>>> [4,5,6]])
>>> v2 = np.array([[1,0,0],
>>> [0,1,0]])
>>> skinematics.vector.angle(v1,v2)
array([ 1.30024656, 0.96453036])
Notes
-----
.. math::
\\alpha =arccos(\\frac{\\vec{v_1} \\cdot \\vec{v_2}}{| \\vec{v_1} |
\\cdot | \\vec{v_2}|})
"""
# make sure lists are handled correctly
v1 = np.array(v1)
v2 = np.array(v2)
if v1.ndim < v2.ndim:
v1, v2 = v2, v1
n1 = normalize(v1)
n2 = normalize(v2)
if v2.ndim == 1:
angle = np.arccos(n1.dot(n2))
else:
angle = np.arccos(list(map(np.dot, n1, n2)))
return angle
[docs]
def project_onto_line(data: np.ndarray, to: np.ndarray) -> np.ndarray:
"""Project one vector onto another.
Parameters
----------
data : array (N,) or (M,N)
original vector(s)
to : array (N,) or (M,N):
target vector(s)
Returns
-------
v_projected : array (N,) or (M,N)
projection of v1 onto (normalized) v2
Notes
-----
* Either 'vec' or 'to' has to be 1-dimensional!
.. math::
\\vec{n} = \\frac{ \\vec{to} }{| \\vec{to} |}
\\vec{v}_{line} = (\\vec{v} \\cdot \\vec{n}) \\vec{n}
.. image:: ../docs/Images/vector_project.png
:scale: 33%
Example
-------
>>> v1 = np.array([[1,0,0],
>>> [1,1,0]])
>>> v2 = np.r_[1,1,1]
>>> skinematics.vector.project_onto_line(v1,v2)
array([[0.33333333 0.33333333 0.33333333]
[0.66666667 0.66666667 0.66666667]])
"""
# if e2.ndim == 1 or e2.shape[0] == 1:
# return (e2 * list(map(np.dot, v1, e2))).ravel()
# else:
# return (e2.T * list(map(np.dot, v1, e2))).T
data = np.atleast_2d(data)
to = np.atleast_2d(to)
to_n = normalize(to)
if to.shape[0] == 1:
projected = (data @ to_n.T) @ to_n
elif data.shape == to.shape:
projected = (to_n.T * list(map(np.dot, data, to_n))).T
else:
raise TypeError('PROJECT_ONTO_LINE: "data" and "to" must have the same shape')
return projected
[docs]
def project_into_plane(vec: np.ndarray, n_plane: np.ndarray) -> np.ndarray:
"""Projection into the plane perpendicular to n, through zero
Parameters
----------
vec : array (3,) or (M,3)
Vector(s) to be projected
n_plane : array (3,)
Vector perpendicular to plane (does not have to be normalized)
Returns
-------
v_projected : array (3,) or (M,3)
Vector(s) projected into the plane
Notes
-----
.. math::
\\vec{n} = \\frac{ \\overrightarrow{n_{plane}}}{| \\overrightarrow{n_{plane}} |}
\\vec{v}_{plane} = \\vec{v} - (\\vec{v} \\cdot \\vec{n})\\vec{n}
"""
vec = np.array(vec)
n_plane = normalize(n_plane)
vec_projected = vec - project_onto_line(vec, n_plane)
return vec_projected
[docs]
@deprecated(version='0.11.0', reason='Use "project_onto_line" or '
'"project_into_plane" instead.')
def project(v1, v2, projection_type="1D"):
"""Deprecated function, calls 'project_onto_line' or 'project_into_plane'.
Project one vector onto another, or into the plane perpendicular to that vector.
Parameters
----------
v1 : array (N,) or (M,N)
projected vector
v2 : array (N,) or (M,N):
target vector
projection_type : scalar
Has to be one of the following:
- 1D ... projection onto a vector (Default)
- 2D ... projection into the plane perpendicular to that vector
Returns
-------
v_projected : array (N,) or (M,N)
projection of v1 onto v2
"""
if projection_type == "1D":
return project_onto_line(v1, v2)
elif projection_type == "2D":
return project_into_plane(v1, v2)
else:
raise ValueError(
"{0} not allowed as projection_type in vector.project!".format(
projection_type
)
)
[docs]
def GramSchmidt(p0, p1, p2, rotmat=False):
"""Gram-Schmidt orthogonalization
Parameters
----------
p0 : array (3,) or (M,3)
coordinates of Point 1:
p1 : array (3,) or (M,3)
coordinates of Point 2
p2 : array (3,) or (M,3)
coordinates of Point 3
rotmat : if 'True', the output corresponds to a rotation matrix;
if 'False' (default), the output corresponds to the inverse of
the rotation matrix (i.e. rows = coordinate unit vectors)
Returns
-------
Rmat : array (9,) or (M,9)
flattened rotation matrix, or its inverse (see option 'rotmat')
.. image:: ../docs/Images/GramSchmidt.jpg
:scale: 25%
Example
-------
>>> P0 = np.array([[0, 0, 0], [1,2,3]])
>>> P1 = np.array([[1, 0, 0], [4,1,0]])
>>> P2 = np.array([[1, 1, 0], [9,-1,1]])
>>> GramSchmidt(P0,P1,P2)
array([[ 1. , 0. , 0. , 0. , 1. ,
0. , 0. , 0. , 1. ],
[ 0.6882472 , -0.22941573, -0.6882472 , 0.62872867, -0.28470732,
0.72363112, -0.36196138, -0.93075784, -0.05170877]])
Notes
-----
The flattened rotation matrix (option: "rotmat=True"!) corresponds to
.. math::
\\mathbf{R} = [ \\vec{e}_1 \\, \\vec{e}_2 \\, \\vec{e}_3 ]
"""
# If inputs are lists, convert them to arrays:
p0 = np.array(p0)
p1 = np.array(p1)
p2 = np.array(p2)
v1 = np.atleast_2d(p1 - p0)
v2 = np.atleast_2d(p2 - p0)
ex = normalize(v1)
ey = normalize(v2 - project_onto_line(v2, ex))
ez = np.cross(ex, ey)
if rotmat:
return np.hstack((ex, ey, ez)).T
else:
return np.hstack((ex, ey, ez))
[docs]
def plane_orientation(p0, p1, p2):
"""The vector perpendicular to the plane defined by three points.
Parameters
----------
p0 : array (3,) or (M,3)
coordinates of Point 0
p1 : array (3,) or (M,3)
coordinates of Point 1
p2 : array (3,) or (M,3)
coordinates of Point 2
Returns
-------
n : array (3,) or (M,3)
vector perpendicular to the plane
.. image:: ../docs/Images/vector_plane_orientation.png
:scale: 33%
Example
-------
>>> P0 = np.array([[0, 0, 0], [1,2,3]])
>>> P1 = np.array([[1, 0, 0], [4,1,0]])
>>> P2 = np.array([[1, 1, 0], [9,-1,1]])
>>> plane_orientation(P0,P1,P2)
array([[ 0. , 0. , 1. ],
[-0.36196138, -0.93075784, -0.05170877]])
Notes
-----
.. math::
\\vec{n} = \\frac{ \\vec{a} \\times \\vec{b}} {| \\vec{a} \\times \\vec{b}|}
"""
# If inputs are lists, convert them to arrays:
p0 = np.array(p0)
p1 = np.array(p1)
p2 = np.array(p2)
v01 = p1 - p0
v02 = p2 - p0
n = np.cross(v01, v02)
return normalize(n)
# @deprecation.deprecated(deprecated_in="1.7", removed_in="1.9",
# current_version=__version__,
# details="Use the ``q_shortest_rotation`` function instead")
[docs]
def q_shortest_rotation(v1, v2):
"""Quaternion indicating the shortest rotation from one vector into another.
You can read "qrotate" as either "quaternion rotate" or as "quick
rotate".
Parameters
----------
v1 : ndarray (3,)
first vector
v2 : ndarray (3,)
second vector
Returns
-------
q : ndarray (3,)
quaternion rotating v1 into v2
.. image:: ../docs/Images/vector_q_shortest_rotation.png
:scale: 33%
Example
-------
>>> v1 = np.r_[1,0,0]
>>> v2 = np.r_[1,1,0]
>>> q = qrotate(v1, v2)
>>> print(q)
[ 0. 0. 0.38268343]
"""
# calculate the direction
n = normalize(np.cross(v1, v2))
# make sure vectors are handled correctly
n = np.atleast_2d(n)
# handle 0-quaternions
nanindex = np.isnan(n[:, 0])
n[nanindex, :] = 0
# find the angle, and calculate the quaternion
angle12 = angle(v1, v2)
q = (n.T * np.sin(angle12 / 2.0)).T
# if you are working with vectors, only return a vector
if q.shape[0] == 1:
q = q.flatten()
return q
[docs]
def rotate_vector(vector, q):
"""
Rotates a vector, according to the given quaternions.
Note that a single vector can be rotated into many orientations;
or a row of vectors can all be rotated by a single quaternion.
Parameters
----------
vector : array, shape (3,) or (N,3)
vector(s) to be rotated.
q : array_like, shape ([3,4],) or (N,[3,4])
quaternions or quaternion vectors.
Returns
-------
rotated : array, shape (3,) or (N,3)
rotated vector(s)
.. image:: ../docs/Images/vector_rotate_vector.png
:scale: 33%
Notes
-----
.. math::
q \\circ \\left( {\\vec x \\cdot \\vec I} \\right) \\circ {q^{ - 1}} = \\left( {{\\bf{R}} \\cdot \\vec x} \\right) \\cdot \\vec I
More info under
http://en.wikipedia.org/wiki/Quaternion
Examples
--------
>>> mymat = eye(3)
>>> myVector = r_[1,0,0]
>>> quats = array([[0,0, sin(0.1)],[0, sin(0.2), 0]])
>>> quat.rotate_vector(myVector, quats)
array([[ 0.98006658, 0.19866933, 0. ],
[ 0.92106099, 0. , -0.38941834]])
>>> quat.rotate_vector(mymat, [0, 0, sin(0.1)])
array([[ 0.98006658, 0.19866933, 0. ],
[-0.19866933, 0.98006658, 0. ],
[ 0. , 0. , 1. ]])
"""
vector = np.atleast_2d(vector)
qvector = np.hstack((np.zeros((vector.shape[0], 1)), vector))
vRotated = quat.q_mult(q, quat.q_mult(qvector, quat.q_inv(q)))
vRotated = vRotated[:, 1:]
if min(vRotated.shape) == 1:
vRotated = vRotated.ravel()
return vRotated
[docs]
def target2orient(target, orient_type="quat"):
"""Converts a target vector into a corresponding orientation.
Useful for targeting devices, such as eyes, cameras, or missile trackers.
Based on the assumption, that in the reference orientation, the targeting
device points forward.
Parameters
----------
target : array (3,) or (N,3)
Input vector
orient_type : string
Has to be one the following:
- Fick ... Rz * Ry
- nautical ... same as "Fick"
- Helmholtz ... Ry * Rz
- quat ... quaternion
Returns
-------
orientation : array (3,) or (N,3)
Corresponding orientation
For rotation matrices, same sequence as the matrices [deg].
For quaternions, the quaternion vector.
Note that the last column of the sequence angles, and the first column
of the quaterion, will always be zero, because a rotation about
the line-of-sight has no effect.
Example
-------
>>> a = [3,3,0]
>>> b = [5., 0, 5]
>>> skinematics.vector.target2orient(a)
[ 0. 0. 0.38268343]
>>> skinematics.vector.target2orient([a,b])
[[ 0. 0. 0.38268343]
[ 0. -0.38268343 0. ]]
>>> skinematics.vector.target2orient(a, orient_type='nautical')
[ 45. -0. 0.]
"""
if orient_type == "quat":
orientation = q_shortest_rotation([1, 0, 0], target)
elif orient_type == "Fick" or orient_type == "nautical":
n = np.atleast_2d(normalize(target))
theta = np.arctan2(n[:, 1], n[:, 0])
phi = -np.arcsin(n[:, 2])
orientation = np.column_stack((theta, phi, np.zeros_like(theta)))
orientation = np.rad2deg(orientation)
elif orient_type == "Helmholtz":
n = np.atleast_2d(normalize(target))
phi = -np.arctan2(n[:, 2], n[:, 0])
theta = np.arcsin(n[:, 1])
orientation = np.column_stack((phi, theta, np.zeros_like(theta)))
orientation = np.rad2deg(orientation)
else:
raise ValueError("Input parameter {0} not known".format(orientation))
# For vector input, return a vector:
if orientation.shape[0] == 1:
orientation = orientation.ravel()
return orientation
if __name__ == "__main__":
v1 = np.r_[1, 0, 0]
v2 = np.r_[1, 1, 0]
v3 = np.r_[1, 1, 1]
data = np.vstack((v1, v2, v3))
print(project_onto_line(v1, v2))
print(project_onto_line(data, v1))
