#!/usr/bin/env python3
# -*- coding: utf-8 -*-
# ----------------------------------------------------------------------
# Copyright 2019-2020 Airinnova AB and the CommonLibs authors
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# ----------------------------------------------------------------------
# Author: Aaron Dettmann
"""
Vectors
"""
import numpy as np
[docs]def unit_vector(vector):
"""Return the unit vector of a vector."""
return vector/np.linalg.norm(vector)
[docs]def direction_cosine(vector1, vector2):
"""
Returns the direcion cosine between vectors 'vector1' and 'vector2'
See also:
* https://en.wikipedia.org/wiki/Euclidean_vector#Conversion_between_multiple_Cartesian_bases
Args:
:vector1: First vector
:vector2: Second vector
Returns:
:direction_cosine: Direction cosine
"""
v1_u = unit_vector(vector1)
v2_u = unit_vector(vector2)
return np.clip(np.dot(v1_u, v2_u), a_min=-1.0, a_max=1.0)
[docs]def angle_between(vector1, vector2):
"""Return the angle in radians between vectors 'vector1' and 'vector2'
# See also: https://stackoverflow.com/questions/2827393/
angles-between-two-n-dimensional-vectors-in-python/13849249#13849249
"""
return np.arccos(direction_cosine(vector1, vector2))
[docs]def axis_rot_matrix(axis, angle):
"""
Return a rotation matrix associated with counterclockwise rotation about a given axis by angle radians.
Note:
* The rotation matrix is computed using the Euler-Rodrigues formula:
https://en.wikipedia.org/wiki/Euler%E2%80%93Rodrigues_formula
Args:
:axis: 3x1 axis vector
:angle: rotation aangle in radians
Returns:
:R: rotation matrix
"""
# Parse and normalise the axis vector
axis = np.asarray(axis)
axis = axis/np.sqrt(np.dot(axis, axis))
a = np.cos(angle/2.0)
b, c, d = -axis*np.sin(angle/2.0)
aa, bb, cc, dd = a*a, b*b, c*c, d*d
bc, ad, ac, ab, bd, cd = b*c, a*d, a*c, a*b, b*d, c*d
return np.array([[aa + bb - cc - dd, 2*(bc + ad), 2*(bd - ac)],
[2*(bc - ad), aa + cc - bb - dd, 2*(cd + ab)],
[2*(bd + ac), 2*(cd - ab), aa + dd - bb - cc]])
[docs]def rotate_vector_around_axis(vector, axis, angle, return_unit=False):
"""
Rotate a vector by an angle around a defined axis.
Note:
* Returns the original vector if the Euclidean norm of 'vector' or
'axis' is zero or if 'angle' is zero
Args:
:vector: Vector to be rotated
:axis: 3x1 axis vector
:angle: Rotation angle in radians
Returns:
:rot_vector: rotated vector
"""
if np.linalg.norm(vector) == 0 or np.linalg.norm(axis) == 0 or angle == 0:
return vector
rot_vector = np.dot(axis_rot_matrix(axis, angle), vector)
if return_unit:
rot_vector = unit_vector(rot_vector)
return rot_vector
[docs]def get_plane_line_intersect(plane_normal, plane_point, line_direction, line_point, epsilon=1e-6):
"""
Get the intersection point for a line and a plane in 3D space.
Note:
* The line is defined by its direction (vector) and any point on the line
* The plane is defined by its normal (vector) and any point on the plane
Args:
:plane_normal: 3x1 normal vector of the plane
:plane_point: point (3x1 vector) of the plane (any)
:line_direction: 3x1 vector with the direction of the line
:line_point: point (3x1 vector) of the line (any)
:epsilon: tolerance for near-orthogonal 'plane_normal' and 'line_direction'
"""
dot_pn_ld = np.dot(plane_normal, line_direction)
# If plane normal and line direction are (nearly) orthogonal, abort
if abs(dot_pn_ld) < epsilon:
raise RuntimeError("No intersection with plane")
from_pp2lp = line_point - plane_point
si = -np.dot(plane_normal, from_pp2lp)/dot_pn_ld
intersect_point = plane_point + from_pp2lp + si*line_direction
return intersect_point
[docs]def vector_projection(a, b):
"""
Return the vector projection from vector a onto vector b
See:
* https://en.wikipedia.org/wiki/Vector_projection
Args:
:a: vector to be projected
:b: vector onto which a is projected
Returns:
:a1: vector projection
"""
a1 = np.dot(a, unit_vector(b))*unit_vector(b)
return a1
[docs]def vector_rejection(a, b):
"""
Return the vector rejection from vector a onto vector b
See:
* https://en.wikipedia.org/wiki/Vector_projection
Args:
:a: vector to be projected
:b: vector onto which a is projected
Returns:
:a2: vector rejection
"""
a1 = vector_projection(a, b)
return a - a1
[docs]def vector_projection_onto_plane(vector, plane_normal):
"""
Project a vecor onto a plane
Args:
:vector: vector to be projected
:plane_normal: normal vector of the plane
Returns:
:vector_proj: projected vector
"""
# The projected vector equal to the vector minus the "vertical"
# component of the vector, i.e. the component which points in
# the plane normal direction. The vertical component is given by
# a vector projection
return vector - vector_projection(vector, plane_normal)