def rotate2d(vec: Vector, ang: Angle):
x = vec[0] * ang.cos() - vec[1] * ang.sin()
y = vec[0] * ang.sin() + vec[1] * ang.cos()
if vec.dimension == 3:
return Vector([x, y, vec[3]])
else:
return Vector([x, y])
def test_side_vector(array_a, array_b, value_expected):
if value_expected is None:
with pytest.raises(ValueError):
Vector(array_a).side_vector(array_b)
else:
assert Vector(array_a).side_vector(array_b) == value_expected
def test_angle_signed(array_u, array_v, angle_expected):
if angle_expected is None:
with pytest.raises(ValueError, match="The vectors must be 2D."):
Vector(array_u).angle_signed(array_v)
else:
angle = Vector(array_u).angle_signed(array_v)
assert math.isclose(angle, angle_expected)
def test_unit(array, array_unit_expected):
if array_unit_expected is None:
with pytest.raises(ValueError,
match="The magnitude must not be zero."):
Vector(array).unit()
else:
assert Vector(array).unit().is_close(array_unit_expected)
def test_cosine_similarity(array_u, array_v, similarity_expected):
if similarity_expected is None:
with pytest.raises(ValueError,
match="The vectors must have non-zero magnitudes."):
Vector(array_u).cosine_similarity(array_v)
else:
similarity = Vector(array_u).cosine_similarity(array_v)
assert math.isclose(similarity, similarity_expected)
def compute_basis(points_stacked: xr.DataArray) -> Tuple[Basis, xr.DataArray]:
"""
Return origin and basis vectors of new coordinate system found with RANSAC.
Parameters
----------
points_stacked : xarray.DataArray
(N_frames, N_dims, N_layers) array of points.
Returns
-------
basis : namedtuple
Basis of new coordinate system (origin point and three unit vectors).
Fields include 'origin', 'forward', 'up', 'perp'.
points_grouped_inlier : xarray.DataArray
(N_frames, N_dims) array.
Grouped foot points that are marked inliers by RANSAC.
"""
frames = points_stacked.coords['frames'].values
points_head = points_stacked.sel(layers='points_head').values
points_a = points_stacked.sel(layers='points_a').values
points_b = points_stacked.sel(layers='points_b').values
points_foot_mean = (points_a + points_b) / 2
vectors_up = points_head - points_foot_mean
vector_up = Vector(np.median(vectors_up, axis=0)).unit()
frames_grouped = np.repeat(frames, 2)
points_grouped = nf.interweave_rows(points_a, points_b)
model_ransac, is_inlier = fit_ransac(points_grouped)
point_origin, vector_forward = model_ransac.params
vector_perp = Vector(vector_up).cross(vector_forward)
frames_grouped_inlier = frames_grouped[is_inlier]
points_grouped_inlier = points_grouped[is_inlier]
points_grouped_inlier = xr.DataArray(
points_grouped_inlier,
coords={
'frames': frames_grouped_inlier,
'cols': range(3)
},
dims=('frames', 'cols'),
)
basis = Basis(point_origin, vector_forward, vector_up, vector_perp)
return basis, points_grouped_inlier
def test_unit(array):
vector = Vector(array)
vector_unit = vector.unit()
assert math.isclose(vector_unit.norm(), 1)
assert (vector.norm() * vector_unit).is_close(array)
assert vector_unit.is_parallel(vector)
angle = vector.angle_between(vector_unit)
assert math.isclose(angle, 0, abs_tol=ATOL)
def test_is_close(array):
vector = Vector(array)
point = Point(array)
assert point.size == vector.size
assert point.is_close(vector)
assert vector.is_close(point)
assert point.is_close(array)
assert vector.is_close(array)
def test_add_subtract(arrays):
array_point, array_vector = arrays
point = Point(array_point)
vector = Vector(array_vector)
point_2 = point + array_vector
assert math.isclose(point.distance_point(point_2), vector.norm())
point_3 = point_2 - array_vector
assert point.is_close(point_3)
def test_scale(array, scalar):
assume(abs(scalar) > ATOL)
vector = Vector(array)
vector_scaled = scalar * vector
assert vector_scaled.is_parallel(array)
angle = vector_scaled.angle_between(array)
if scalar > 0:
assert math.isclose(angle, 0, abs_tol=ATOL)
else:
assert math.isclose(angle, np.pi, rel_tol=1e-6)
def vectors_nonzero(draw, dim):
"""
Return a strategy which generates nonzero Vector objects.
Parameters
----------
dim : int
Dimension of the object.
Returns
-------
LazyStrategy
Hypothesis strategy.
"""
return Vector(draw(arrays_fixed_nonzero(dim)))
def test_two_vectors(arrays):
array_a, array_b = arrays
vector_a = Vector(array_a)
is_perpendicular = vector_a.is_perpendicular(array_b)
is_parallel = vector_a.is_parallel(array_b)
# Two non-zero vectors cannot be both perpendicular and parallel.
assert not (is_perpendicular and is_parallel)
angle = vector_a.angle_between(array_b)
if is_perpendicular:
assert math.isclose(angle, np.pi / 2)
if is_parallel:
assert math.isclose(angle, 0, abs_tol=ATOL) or math.isclose(
angle, np.pi, rel_tol=1e-6)
# The zero vector is perpendicular and parallel to any other vector.
vector_zero = np.zeros(vector_a.size)
assert vector_a.is_perpendicular(vector_zero)
assert vector_a.is_parallel(vector_zero)
# The angle with the zero vector is undefined.
message_expected = "The vectors must have non-zero magnitudes."
with pytest.raises(ValueError, match=message_expected):
vector_a.angle_between(vector_zero)
# The projection of vector B onto A is parallel to A.
vector_b_projected = vector_a.project_vector(array_b)
assert vector_a.is_parallel(vector_b_projected)
# The projection is zero if vectors A and B are perpendicular.
if is_perpendicular:
assert vector_b_projected.is_zero(abs_tol=ATOL)
def vectors(draw, dim):
"""
Return a strategy which generates Vector objects.
Parameters
----------
dim : int
Dimension of the object.
Returns
-------
LazyStrategy
Hypothesis strategy.
Examples
--------
>>> from hypothesis import find
>>> from tests.property.strategies import vectors
>>> find(vectors(2), lambda x: True)
Vector([0., 0.])
"""
return Vector(draw(arrays_fixed(dim)))
"""
Vector-Plane Projection
=======================
Project a vector onto a plane.
"""
from skspatial.objects import Plane
from skspatial.objects import Vector
from skspatial.plotting import plot_3d
plane = Plane([0, 0, 0], [0, 0, 1])
vector = Vector([1, 1, 1])
vector_projected = plane.project_vector(vector)
_, ax = plot_3d(
plane.plotter(lims_x=(-5, 5), lims_y=(-5, 5), alpha=0.3),
vector.plotter(point=plane.point, color='k'),
vector_projected.plotter(point=plane.point,
color='r',
linewidth=2,
zorder=3),
)
ax.set_zlim([-1, 1])
residual_threshold=2.5 * mad(points[:, 2], c=1),
)
return model, is_inlier
@require(
"The layers must include head and two feet.",
lambda args: set(args.points_stacked.layers.values) ==
{'points_a', 'points_b', 'points_head'},
)
@ensure(
# This contract assumes an orientation where x = length along walkway, z = depth.
# It can be removed if new data does not have this orientation.
"The perpendicular vector must be to the right of the forward vector.",
lambda _, result: Vector(result[0].forward[[0, 2]]).side_vector(result[
0].perp[[0, 2]]) == 1,
)
def compute_basis(points_stacked: xr.DataArray) -> Tuple[Basis, xr.DataArray]:
"""
Return origin and basis vectors of new coordinate system found with RANSAC.
Parameters
----------
points_stacked : xarray.DataArray
(N_frames, N_dims, N_layers) array of points.
Returns
-------
basis : namedtuple
Basis of new coordinate system (origin point and three unit vectors).
Fields include 'origin', 'forward', 'up', 'perp'.
from skspatial.objects import Plane
from skspatial.objects import Point
from skspatial.objects import Points
from skspatial.objects import Sphere
from skspatial.objects import Triangle
from skspatial.objects import Vector
@pytest.mark.parametrize(
("obj_spatial", "repr_expected"),
[
(Point([0]), "Point([0])"),
(Point([0, 0]), "Point([0, 0])"),
(Point([0.5, 0]), "Point([0.5, 0. ])"),
(Point([-11, 0]), "Point([-11, 0])"),
(Vector([-11, 0]), "Vector([-11, 0])"),
(Vector([-11.0, 0.0]), "Vector([-11., 0.])"),
(Vector([0, 0]), "Vector([0, 0])"),
(Vector([0.5, 0]), "Vector([0.5, 0. ])"),
(Points([[1.5, 2], [5, 3]]), "Points([[1.5, 2. ],\n [5. , 3. ]])"),
(Line([0, 0], [1, 0]), "Line(point=Point([0, 0]), direction=Vector([1, 0]))"),
(Line([-1, 2, 3], [5, 4, 2]), "Line(point=Point([-1, 2, 3]), direction=Vector([5, 4, 2]))"),
(Line(np.zeros(2), [1, 0]), "Line(point=Point([0., 0.]), direction=Vector([1, 0]))"),
(Plane([0, 0], [1, 0]), "Plane(point=Point([0, 0]), normal=Vector([1, 0]))"),
(Plane([-1, 2, 3], [5, 4, 2]), "Plane(point=Point([-1, 2, 3]), normal=Vector([5, 4, 2]))"),
(Circle([0, 0], 1), "Circle(point=Point([0, 0]), radius=1)"),
(Circle([0, 0], 2.5), "Circle(point=Point([0, 0]), radius=2.5)"),
(Sphere([0, 0, 0], 1), "Sphere(point=Point([0, 0, 0]), radius=1)"),
(
Triangle([0, 0], [0, 1], [1, 0]),
"Triangle(point_a=Point([0, 0]), point_b=Point([0, 1]), point_c=Point([1, 0]))",
"""
3D Vector-Line Projection
=========================
Project a vector onto a line.
"""
from skspatial.objects import Vector, Line
from skspatial.plotting import plot_3d
line = Line([0, 0, 0], [1, 1, 2])
vector = Vector([1, 1, 0.1])
vector_projected = line.project_vector(vector)
plot_3d(
line.plotter(t_1=-1, c='k', linestyle='--'),
vector.plotter(point=line.point, color='k'),
vector_projected.plotter(point=line.point,
color='r',
linewidth=2,
zorder=3),
)
def test_add_subtract(array):
vector = Vector(array)
assert (vector + array - array).is_close(array)
def test_angle_between(array_u, array_v, angle_expected):
"""Test finding the angle between vectors u and v."""
angle = Vector(array_u).angle_between(array_v)
assert math.isclose(angle, angle_expected)
def test_side_vector_failure(array_a, array_b):
message_expected = "The vectors must be 2D."
with pytest.raises(ValueError, match=message_expected):
Vector(array_a).side_vector(array_b)
def test_side_vector(array_a, array_b, value_expected):
assert Vector(array_a).side_vector(array_b) == value_expected
def test_is_parallel(array_u, array_v, bool_expected):
"""Test checking if vector u is parallel to vector v."""
vector_u = Vector(array_u)
assert vector_u.is_parallel(array_v) == bool_expected
def test_different_direction(array, array_expected):
vector = Vector(array)
vector_expected = Vector(array_expected)
assert vector.different_direction().is_equal(vector_expected)
def test_is_perpendicular(array_u, array_v, bool_expected):
"""Test checking if vector u is perpendicular to vector v."""
vector_u = Vector(array_u)
assert vector_u.is_perpendicular(array_v) == bool_expected
"""
2D Vector-Vector Projection
===========================
Project a vector onto another vector.
"""
from skspatial.objects import Vector
from skspatial.plotting import plot_2d
vector_a = Vector([1, 1])
vector_b = Vector([2, 0])
vector_projected = vector_b.project_vector(vector_a)
_, ax = plot_2d(
vector_a.plotter(color='k', head_width=0.1),
vector_b.plotter(color='k', head_width=0.1),
vector_projected.plotter(color='r', head_width=0.1),
)
ax.axis([-0.5, 2.5, -0.5, 1.5])
def test_different_direction_failure(array):
message_expected = "The vector must not be the zero vector."
with pytest.raises(ValueError, match=message_expected):
Vector(array).different_direction()
def test_is_zero(array, kwargs, bool_expected):
assert Vector(array).is_zero(**kwargs) == bool_expected
import math
import pytest
from numpy.testing import assert_array_equal
from skspatial.objects import Vector
@pytest.mark.parametrize(
"array_a, array_b, vector_expected",
[
([0, 0], [1, 0], Vector([1, 0])),
([1, 0], [1, 0], Vector([0, 0])),
([1, 0], [2, 0], Vector([1, 0])),
([8, 3, -5], [3, 7, 1], Vector([-5, 4, 6])),
([5, 7, 8, 9], [2, 5, 3, -4], Vector([-3, -2, -5, -13])),
],
)
def test_from_points(array_a, array_b, vector_expected):
assert_array_equal(Vector.from_points(array_a, array_b), vector_expected)
@pytest.mark.parametrize(
"array, array_unit_expected",
[
([1, 0], [1, 0]),
([2, 0], [1, 0]),
([-1, 0], [-1, 0]),
([0, 0, 5], [0, 0, 1]),
([1, 1], [math.sqrt(2) / 2, math.sqrt(2) / 2]),
def test_project_vector(vector_u, vector_v, vector_expected):
"""Test projecting vector u onto vector v."""
vector_u_projected = Vector(vector_v).project_vector(vector_u)
assert vector_u_projected.is_close(vector_expected)
def test_equality(array):
assert_array_equal(array, Point(array))
assert_array_equal(array, Vector(array))
assert_array_equal(array, np.array(array))