diff --git a/CHANGELOG.md b/CHANGELOG.md index 799d82d06581..a22d19587ef0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,8 +9,21 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ### Added +- Added `matrix_change_basis`, `Transformation.change_basis` +- Added `matrix_from_frame_to_frame` +- Added non-numpy versions of `global_coords`, `local_coords` +- Added static method `Frame.local_to_local_coords` +- Added `__getitem__`, `__setitem__` and `__eq__` to `Quaternion` +- Added `Vector.scaled` and `Vector.unitized` +- Added `transform_frames` and respective helper functions `dehomogenize_and_unflatten_frames`, `homogenize_and_flatten_frames` +- Added `transform_frames_numpy` and respective helper functions `dehomogenize_and_unflatten_frames_numpy`, `homogenize_and_flatten_frames_numpy` + ### Changed +- Changed parameters `origin` `uvw` of `global_coords_numpy` and `local_coords_numpy` to `frame` +- Fixed some returns of `Frame` and `Rotation` to use `Vector` or `Quaternion` +- Methods `Frame.represent_point/vector/frame_in_global_coordinates` and `Frame.represent_point/vector/frame_in_local_coordinates` are now `Frame.local_coords` and `Frame.global_coords` + ### Removed diff --git a/src/compas/geometry/basic.py b/src/compas/geometry/basic.py index 62c2ae433052..91b364bc7198 100644 --- a/src/compas/geometry/basic.py +++ b/src/compas/geometry/basic.py @@ -8,6 +8,7 @@ from random import uniform __all__ = [ + 'close', 'allclose', 'add_vectors', 'add_vectors_xy', @@ -59,6 +60,27 @@ ] +def close(value1, value2, tol=1e-05): + """Returns True if two values are equal within a tolerance. + + Parameters + ---------- + value1 : float or int + value2 : float or int + tol : float, optional + The tolerance for comparing values. + Default is ``1e-05``. + + Examples + -------- + >>> close(1., 1.001) + False + >>> close(1., 1.001, tol=1e-2) + True + """ + return fabs(value1 - value2) < tol + + def allclose(l1, l2, tol=1e-05): """Returns True if two lists are element-wise equal within a tolerance. diff --git a/src/compas/geometry/bbox/bbox_numpy.py b/src/compas/geometry/bbox/bbox_numpy.py index 1822cb712265..c952c5e27384 100644 --- a/src/compas/geometry/bbox/bbox_numpy.py +++ b/src/compas/geometry/bbox/bbox_numpy.py @@ -16,8 +16,8 @@ # from scipy.spatial import QhullError from compas.geometry import local_axes -from compas.geometry import local_coords_numpy -from compas.geometry import global_coords_numpy +from compas.geometry import world_to_local_coords_numpy +from compas.geometry import local_to_world_coords_numpy __all__ = [ @@ -27,7 +27,7 @@ def oriented_bounding_box_numpy(points): - """Compute the oriented minimum bounding box of a set of points in 3D space. + r"""Compute the oriented minimum bounding box of a set of points in 3D space. Parameters ---------- @@ -85,8 +85,8 @@ def oriented_bounding_box_numpy(points): >>> a = length_vector(subtract_vectors(bbox[1], bbox[0])) >>> b = length_vector(subtract_vectors(bbox[3], bbox[0])) >>> c = length_vector(subtract_vectors(bbox[4], bbox[0])) - >>> a * b * c - 30.0 + >>> close(a * b * c, 30.) + True """ points = asarray(points) @@ -114,7 +114,8 @@ def oriented_bounding_box_numpy(points): a, b, c = points[simplex] uvw = local_axes(a, b, c) xyz = points[hull.vertices] - rst = local_coords_numpy(a, uvw, xyz) + frame = [a, uvw[0], uvw[1]] + rst = world_to_local_coords_numpy(frame, xyz) dr, ds, dt = ptp(rst, axis=0) v = dr * ds * dt @@ -131,7 +132,7 @@ def oriented_bounding_box_numpy(points): [rmax, smax, tmax], [rmin, smax, tmax], ] - bbox = global_coords_numpy(a, uvw, bbox) + bbox = local_to_world_coords_numpy(frame, bbox) volume = v return bbox @@ -191,7 +192,7 @@ def oriented_bounding_box_xy_numpy(points): p1 = points[simplex[1]] # s direction - s = p1 - p0 + s = p1 - p0 sl = sum(s ** 2) ** 0.5 su = s / sl vn = xy_hull - p0 @@ -204,7 +205,7 @@ def oriented_bounding_box_xy_numpy(points): b1 = p0 + sc[scmax] * su # t direction - t = array([-s[1], s[0]]) + t = array([-s[1], s[0]]) tl = sum(t ** 2) ** 0.5 tu = t / tl vn = xy_hull - p0 @@ -239,35 +240,15 @@ def oriented_bounding_box_xy_numpy(points): if __name__ == "__main__": import numpy + import math from compas.geometry import bounding_box from compas.geometry import subtract_vectors from compas.geometry import length_vector from compas.geometry import Rotation from compas.geometry import transform_points_numpy from compas.geometry import allclose + from compas.geometry import close - points = numpy.random.rand(10000, 3) - bottom = numpy.array([[0.0,0.0,0.0], [1.0,0.0,0.0], [0.0,1.0,0.0], [1.0,1.0,0.0]]) - top = numpy.array([[0.0, 0.0, 1.0], [1.0, 0.0, 1.0], [0.0, 1.0, 1.0], [1.0, 1.0, 1.0]]) - points = numpy.concatenate((points, bottom, top)) - points[:, 0] *= 10 - points[:, 2] *= 3 + import doctest + doctest.testmod(globs=globals()) - bbox = bounding_box(points) - a = length_vector(subtract_vectors(bbox[1], bbox[0])) - b = length_vector(subtract_vectors(bbox[3], bbox[0])) - c = length_vector(subtract_vectors(bbox[4], bbox[0])) - v1 = a * b * c - - R = Rotation.from_axis_and_angle([1.0, 1.0, 0.0], 0.5 * 3.14159) - points = transform_points_numpy(points, R.matrix) - - bbox = oriented_bounding_box_numpy(points) - - a = length_vector(subtract_vectors(bbox[1], bbox[0])) - b = length_vector(subtract_vectors(bbox[3], bbox[0])) - c = length_vector(subtract_vectors(bbox[4], bbox[0])) - v2 = a * b * c - - print(v1, v2) - print(allclose([v1], [v2])) diff --git a/src/compas/geometry/bestfit/bestfit_numpy.py b/src/compas/geometry/bestfit/bestfit_numpy.py index 1b74668723ff..28229122e370 100644 --- a/src/compas/geometry/bestfit/bestfit_numpy.py +++ b/src/compas/geometry/bestfit/bestfit_numpy.py @@ -13,8 +13,8 @@ from scipy.optimize import leastsq # should this not be defined in a different location? -from compas.geometry import local_coords_numpy -from compas.geometry import global_coords_numpy +from compas.geometry import world_to_local_coords_numpy +from compas.geometry import local_to_world_coords_numpy from compas.numerical import pca_numpy @@ -151,7 +151,8 @@ def bestfit_circle_numpy(points): """ o, uvw, _ = pca_numpy(points) - rst = local_coords_numpy(o, uvw, points) + frame = [o, uvw[1], uvw[2]] + rst = world_to_local_coords_numpy(frame, points) x = rst[:, 0] y = rst[:, 1] @@ -172,7 +173,7 @@ def f(c): print(residu) - xyz = global_coords_numpy(o, uvw, [[c[0], c[1], 0.0]])[0] + xyz = local_to_world_coords_numpy(frame, [[c[0], c[1], 0.0]])[0] o = xyz.tolist() u, v, w = uvw.tolist() diff --git a/src/compas/geometry/primitives/__init__.py b/src/compas/geometry/primitives/__init__.py index 3fc329583436..c7ddd094d2ad 100644 --- a/src/compas/geometry/primitives/__init__.py +++ b/src/compas/geometry/primitives/__init__.py @@ -7,6 +7,7 @@ from .point import Point from .line import Line from .plane import Plane +from .quaternion import Quaternion from .frame import Frame from .polyline import Polyline @@ -14,7 +15,6 @@ from .circle import Circle from .curve import Bezier -from .quaternion import Quaternion from .shapes import * diff --git a/src/compas/geometry/primitives/frame.py b/src/compas/geometry/primitives/frame.py index 9bbced64bda8..c8097c08a893 100644 --- a/src/compas/geometry/primitives/frame.py +++ b/src/compas/geometry/primitives/frame.py @@ -25,6 +25,7 @@ from compas.geometry.primitives import Point from compas.geometry.primitives import Vector from compas.geometry.primitives import Plane +from compas.geometry.primitives import Quaternion __all__ = ['Frame'] @@ -530,16 +531,16 @@ def zaxis(self): @property def quaternion(self): - """:obj:`list` of :obj:`float` : The 4 quaternion coefficients from the rotation given by the frame. + """:class:`Quaternion` : The quaternion from the rotation given by the frame. """ rotation = matrix_from_basis_vectors(self.xaxis, self.yaxis) - return quaternion_from_matrix(rotation) + return Quaternion(*quaternion_from_matrix(rotation)) @property def axis_angle_vector(self): """vector : The axis-angle vector from the rotation given by the frame.""" R = matrix_from_basis_vectors(self.xaxis, self.yaxis) - return axis_angle_vector_from_matrix(R) + return Vector(*axis_angle_vector_from_matrix(R)) # ========================================================================== # representation @@ -648,187 +649,107 @@ def euler_angles(self, static=True, axes='xyz'): R = matrix_from_basis_vectors(self.xaxis, self.yaxis) return euler_angles_from_matrix(R, static, axes) - def represent_point_in_local_coordinates(self, point): - """Represents a point in the frame's local coordinate system. - - Parameters - ---------- - point : :obj:`list` of :obj:`float` or :class:`Point` - A point in world XY. - - Returns - ------- - :class:`Point` - A point in the local coordinate system of the frame. - - Examples - -------- - >>> from compas.geometry import Frame - >>> f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) - >>> pw1 = [2, 2, 2] - >>> pf = f.represent_point_in_local_coordinates(pw1) - >>> pw2 = f.represent_point_in_global_coordinates(pf) - >>> allclose(pw1, pw2) - True - - """ - pt = Point(*subtract_vectors(point, self.point)) - T = inverse(matrix_from_basis_vectors(self.xaxis, self.yaxis)) - pt.transform(T) - return pt + # ========================================================================== + # coordinate frames + # ========================================================================== - def represent_point_in_global_coordinates(self, point): - """Represents a point from local coordinates in the world coordinate system. + def to_local_coords(self, object_in_wcf): + """Returns the object's coordinates in the local coordinate system of the frame. Parameters ---------- - point : :obj:`list` of :obj:`float` or :class:`Point` - A point in local coordinates. + object_in_wcf : :class:`Point` or :class:`Vector` or :class:`Frame` or list of float + An object in the world coordinate frame. Returns ------- - :class:`Point` - A point in the world coordinate system. + :class:`Point` or :class:`Vector` or :class:`Frame` + The object in the local coordinate system of the frame. - Examples - -------- - >>> from compas.geometry import Frame - >>> f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) - >>> pw1 = [2, 2, 2] - >>> pf = f.represent_point_in_local_coordinates(pw1) - >>> pw2 = f.represent_point_in_global_coordinates(pf) - >>> allclose(pw1, pw2) - True - - """ - T = matrix_from_frame(self) - pt = Point(*point) - pt.transform(T) - return pt - - def represent_vector_in_local_coordinates(self, vector): - """Represents a vector in the frame's local coordinate system. - - Parameters - ---------- - vector : :obj:`list` of :obj:`float` or :class:`Vector` - A vector in world XY. - - Returns - ------- - :class:`Vector` - A vector in the local coordinate system of the frame. + Notes + ----- + If you pass a list of float, it is assumed to represent a point. Examples -------- - >>> from compas.geometry import Frame - >>> f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) - >>> pw1 = [2, 2, 2] - >>> pf = f.represent_vector_in_local_coordinates(pw1) - >>> pw2 = f.represent_vector_in_global_coordinates(pf) - >>> allclose(pw1, pw2) - True - + >>> from compas.geometry import Point, Frame + >>> frame = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) + >>> pw = Point(2, 2, 2) # point in wcf + >>> pl = frame.to_local_coords(pw) # point in frame + >>> frame.to_world_coords(pl) + Point(2.000, 2.000, 2.000) """ - T = inverse(matrix_from_basis_vectors(self.xaxis, self.yaxis)) - vec = Vector(*vector) - vec.transform(T) - return vec + T = Transformation.change_basis(Frame.worldXY(), self) + if isinstance(object_in_wcf, list): + return Point(*object_in_wcf).transformed(T) + else: + return object_in_wcf.transformed(T) - def represent_vector_in_global_coordinates(self, vector): - """Represents a vector in local coordinates in the world coordinate system. + def to_world_coords(self, object_in_lcs): + """Returns the object's coordinates in the global coordinate frame. Parameters ---------- - vector: :obj:`list` of :obj:`float` or :class:`Vector` - A vector in local coordinates. + object_in_lcs : :class:`Point` or :class:`Vector` or :class:`Frame` or list of float + An object in local coordinate system of the frame. Returns ------- - :class:`Vector` - A vector in the world coordinate system. - - Examples - -------- - >>> from compas.geometry import Frame - >>> f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) - >>> pw1 = [2, 2, 2] - >>> pf = f.represent_vector_in_local_coordinates(pw1) - >>> pw2 = f.represent_vector_in_global_coordinates(pf) - >>> allclose(pw1, pw2) - True + :class:`Point` or :class:`Vector` or :class:`Frame` + The object in the world coordinate frame. - """ - T = matrix_from_frame(self) - vec = Vector(*vector) - vec.transform(T) - return vec - - def represent_frame_in_local_coordinates(self, frame): - """Represents another frame in the frame's local coordinate system. - - Parameters - ---------- - frame: :class:`Frame` - A frame in the world coordinate system. - - Returns - ------- - :class:`Frame` - A frame in the frame's local coordinate system. + Notes + ----- + If you pass a list of float, it is assumed to represent a point. Examples -------- - >>> f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) - >>> pw1 = Frame([1, 1, 1], [0.707, 0.707, 0], [-0.707, 0.707, 0]) - >>> pf = f.represent_frame_in_local_coordinates(pw1) - >>> pw2 = f.represent_frame_in_global_coordinates(pf) - >>> allclose(pw1.point, pw2.point) - True - >>> allclose(pw1.xaxis, pw2.xaxis) - True - >>> allclose(pw1.yaxis, pw2.yaxis) - True - + >>> from compas.geometry import Frame + >>> frame = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) + >>> pl = Point(1.632, -0.090, 0.573) # point in frame + >>> pw = frame.to_world_coords(pl) # point in wcf + >>> frame.to_local_coords(pw) + Point(1.632, -0.090, 0.573) """ - T = Transformation.from_frame(self).inverse() - f = frame.copy() - f.transform(T) - return f + T = Transformation.change_basis(self, Frame.worldXY()) + if isinstance(object_in_lcs, list): + return Point(*object_in_lcs).transformed(T) + else: + return object_in_lcs.transformed(T) - def represent_frame_in_global_coordinates(self, frame): - """Represents another frame in the local coordinate system in the world - coordinate system. + @staticmethod + def local_to_local_coords(frame1, frame2, object_in_frame1): + """Returns the object's coordinates in frame1 in the local coordinates of frame2. Parameters ---------- - frame: :class:`Frame` - A frame in the local coordinate system. + frame1 : :class:`Frame` + A frame representing one local coordinate system. + frame2 : :class:`Frame` + A frame representing another local coordinate system. + object_in_frame1 : :class:`Point` or :class:`Vector` or :class:`Frame` or list of float + An object in the coordinate frame1. If you pass a list of float, it is assumed to represent a point. Returns ------- - :class:`Frame` - A frame in the world coordinate system. + :class:`Point` or :class:`Vector` or :class:`Frame` + The object in the local coordinate system of frame2. Examples -------- - >>> from compas.geometry import Frame - >>> f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) - >>> pw1 = Frame([1, 1, 1], [0.707, 0.707, 0], [-0.707, 0.707, 0]) - >>> pf = f.represent_frame_in_local_coordinates(pw1) - >>> pw2 = f.represent_frame_in_global_coordinates(pf) - >>> allclose(pw1.point, pw2.point) - True - >>> allclose(pw1.xaxis, pw2.xaxis) - True - >>> allclose(pw1.yaxis, pw2.yaxis) - True - + >>> from compas.geometry import Point, Frame + >>> frame1 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) + >>> frame2 = Frame([2, 1, 3], [1., 0., 0.], [0., 1., 0.]) + >>> p1 = Point(2, 2, 2) # point in frame1 + >>> p2 = Frame.local_to_local_coords(frame1, frame2, p1) # point in frame2 + >>> Frame.local_to_local_coords(frame2, frame1, p2) + Point(2.000, 2.000, 2.000) """ - T = Transformation.from_frame(self) - f = frame.copy() - f.transform(T) - return f + T = Transformation.change_basis(frame1, frame2) + if isinstance(object_in_frame1, list): + return Point(*object_in_frame1).transformed(T) + else: + return object_in_frame1.transformed(T) # ========================================================================== # transformations diff --git a/src/compas/geometry/primitives/primitive.py b/src/compas/geometry/primitives/primitive.py index 429c28b6062f..995ced3682bf 100644 --- a/src/compas/geometry/primitives/primitive.py +++ b/src/compas/geometry/primitives/primitive.py @@ -2,8 +2,6 @@ from __future__ import division from __future__ import print_function -from compas.geometry import Transformation - __all__ = ['Primitive'] diff --git a/src/compas/geometry/primitives/quaternion.py b/src/compas/geometry/primitives/quaternion.py index 4562638ac14e..ce26199f5383 100644 --- a/src/compas/geometry/primitives/quaternion.py +++ b/src/compas/geometry/primitives/quaternion.py @@ -1,3 +1,5 @@ +import math + from compas.geometry import quaternion_multiply from compas.geometry import quaternion_conjugate from compas.geometry import quaternion_unitize @@ -5,7 +7,6 @@ from compas.geometry import quaternion_norm from compas.geometry import quaternion_is_unit -from compas.geometry import allclose from compas.geometry.primitives import Primitive @@ -93,10 +94,7 @@ def __init__(self, w, x, y, z): self.z = float(z) def __iter__(self): - return iter([self.w, self.x, self.y, self.z]) - - def __str__(self): - return "Quaternion = %s" % list(self) + return iter(self.wxyz) def __repr__(self): return 'Quaternion({:.{prec}f}, {:.{prec}f}, {:.{prec}f}, {:.{prec}f})'.format(*self, prec=6) @@ -213,11 +211,50 @@ def conjugate(self): qc = quaternion_conjugate(self) return Quaternion(*qc) + # ========================================================================== + # access + # ========================================================================== + + def __getitem__(self, key): + if key == 0: + return self.w + if key == 1: + return self.x + if key == 2: + return self.y + if key == 3: + return self.z + raise KeyError + + def __setitem__(self, key, value): + if key == 0: + self.w = value + return + if key == 1: + self.x = value + return + if key == 2: + self.y = value + if key == 3: + self.z = value + raise KeyError + + # ========================================================================== + # comparison + # ========================================================================== + + def __eq__(self, other, tol=1e-05): + for v1, v2 in zip(self, other): + if math.fabs(v1 - v2) > tol: + return False + return True + # ============================================================================== # Main # ============================================================================== if __name__ == "__main__": + from compas.geometry import allclose import doctest doctest.testmod(globs=globals()) diff --git a/src/compas/geometry/primitives/vector.py b/src/compas/geometry/primitives/vector.py index 4740fbf7f631..2c288d359fef 100644 --- a/src/compas/geometry/primitives/vector.py +++ b/src/compas/geometry/primitives/vector.py @@ -444,6 +444,18 @@ def unitize(self): self.y = self.y / l self.z = self.z / l + def unitized(self): + """Returns a unitized copy of this ``Vector``. + + Returns + ------- + :class:`Vector` + + """ + v = self.copy() + v.unitize() + return v + def scale(self, n): """Scale this ``Vector`` by a factor n. @@ -457,6 +469,23 @@ def scale(self, n): self.y *= n self.z *= n + def scaled(self, n): + """Returns a scaled copy of this ``Vector``. + + Parameters + ---------- + n : float + The scaling factor. + + Returns + ------- + :class:`Vector` + + """ + v = self.copy() + v.scale(n) + return v + def dot(self, other): """The dot product of this ``Vector`` and another vector. diff --git a/src/compas/geometry/transformations/matrices.py b/src/compas/geometry/transformations/matrices.py index 4fbcf1e9ce66..d440982d52d1 100644 --- a/src/compas/geometry/transformations/matrices.py +++ b/src/compas/geometry/transformations/matrices.py @@ -14,15 +14,14 @@ from compas.geometry.basic import multiply_matrix_vector from compas.geometry.basic import length_vector from compas.geometry.basic import allclose -from compas.geometry.basic import transpose_matrix from compas.geometry.basic import multiply_matrices +from compas.geometry.basic import transpose_matrix from compas.geometry.basic import norm_vector from compas.geometry.transformations import _EPS from compas.geometry.transformations import _SPEC2TUPLE from compas.geometry.transformations import _NEXT_SPEC - __all__ = [ 'matrix_determinant', 'matrix_inverse', @@ -32,6 +31,8 @@ 'identity_matrix', 'matrix_from_frame', + 'matrix_from_frame_to_frame', + 'matrix_change_basis', 'matrix_from_euler_angles', 'matrix_from_axis_and_angle', 'matrix_from_axis_angle_vector', @@ -61,6 +62,7 @@ 'translation_from_matrix', ] + def matrix_determinant(M, check=True): """Calculates the determinant of a square matrix M. @@ -397,6 +399,55 @@ def matrix_from_frame(frame): return M +def matrix_from_frame_to_frame(frame_from, frame_to): + """Computes a transformation between two frames. + + This transformation allows to transform geometry from one Cartesian + coordinate system defined by "frame_from" to another Cartesian + coordinate system defined by "frame_to". + + Parameters + ---------- + frame_from : :class:`Frame` + A frame defining the original Cartesian coordinate system + frame_to : :class:`Frame` + A frame defining the targeted Cartesian coordinate system + + Examples + -------- + >>> f1 = Frame([2, 2, 2], [0.12, 0.58, 0.81], [-0.80, 0.53, -0.26]) + >>> f2 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) + >>> T = matrix_from_frame_to_frame(f1, f2) + """ + T1 = matrix_from_frame(frame_from) + T2 = matrix_from_frame(frame_to) + return multiply_matrices(T2, matrix_inverse(T1)) + + +def matrix_change_basis(frame_from, frame_to): + """Computes a change of basis transformation between two frames. + + A basis change is essentially a remapping of geometry from one + coordinate system to another. + + Parameters + ---------- + frame_from : :class:`Frame` + A frame defining the original Cartesian coordinate system + frame_to : :class:`Frame` + A frame defining the targeted Cartesian coordinate system + + Example: + >>> from compas.geometry import Point, Frame + >>> f1 = Frame([2, 2, 2], [0.12, 0.58, 0.81], [-0.80, 0.53, -0.26]) + >>> f2 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) + >>> T = matrix_change_basis(f1, f2) + """ + T1 = matrix_from_frame(frame_from) + T2 = matrix_from_frame(frame_to) + return multiply_matrices(matrix_inverse(T2), T1) + + def matrix_from_euler_angles(euler_angles, static=True, axes='xyz'): """Calculates a rotation matrix from Euler angles. @@ -1281,5 +1332,7 @@ def axis_angle_from_quaternion(q): # ============================================================================== if __name__ == "__main__": - - pass + import doctest + from compas.geometry import Frame + from compas.geometry import Transformation + doctest.testmod(globs=globals()) diff --git a/src/compas/geometry/transformations/rotation.py b/src/compas/geometry/transformations/rotation.py index 733ffce5c52e..e405a76e52ae 100644 --- a/src/compas/geometry/transformations/rotation.py +++ b/src/compas/geometry/transformations/rotation.py @@ -62,9 +62,9 @@ def from_basis_vectors(cls, xaxis, yaxis): Parameters ---------- - xaxis : :obj:`list` of :obj:`float` + xaxis : :class:`Vector` The x-axis of the frame. - yaxis : :obj:`list` of :obj:`float` + yaxis : :class:`Vector` The y-axis of the frame. Examples @@ -116,7 +116,7 @@ def from_quaternion(cls, quaternion): Parameters ---------- - quaternion : :obj:`list` of :obj:`float` + quaternion : :class:`Quaternion` Four numbers that represents the four coefficient values of a quaternion. Examples @@ -136,10 +136,9 @@ def from_axis_angle_vector(cls, axis_angle_vector, point=[0, 0, 0]): Parameters ---------- - axis_angle_vector : :obj:`list` of :obj:`float` - Three numbers that represent the axis of rotation and angle of rotation - through the vector's magnitude. - point : :obj:`list` of :obj:`float`, optional + axis_angle_vector : list of float + Three numbers that represent the axis of rotation and angle of rotation through the vector's magnitude. + point : list of float, optional A point to perform a rotation around an origin other than [0, 0, 0]. Examples @@ -157,16 +156,19 @@ def from_axis_angle_vector(cls, axis_angle_vector, point=[0, 0, 0]): @classmethod def from_axis_and_angle(cls, axis, angle, point=[0, 0, 0]): - """Calculates a ``Rotation`` from a rotation axis and an angle and \ - an optional point of rotation. + """Calculates a ``Rotation`` from a rotation axis and an angle and an optional point of rotation. + + The rotation is based on the right hand rule, i.e. anti-clockwise if the + axis of rotation points towards the observer. Parameters ---------- - axis (:obj:`list` of :obj:`float`): Three numbers that represent - the axis of rotation - angle (:obj:`float`): The rotation angle in radians. - point (:obj:`list` of :obj:`float`, optional): A point to - perform a rotation around an origin other than [0, 0, 0]. + axis : list of float + Three numbers that represent the axis of rotation. + angle : float + The rotation angle in radians. + point : :class:`Point` or list of float + A point to perform a rotation around an origin other than [0, 0, 0]. Examples -------- @@ -200,12 +202,13 @@ def from_euler_angles(cls, euler_angles, static=True, axes='xyz'): Parameters ---------- - euler_angles : :obj:`list` of :obj:`float` - Three numbers that represent the angles of rotations about the defined axes. - static : :obj:`bool`, optional - If true the rotations are applied to a static frame. - If not, to a rotational. Defaults to true. - axes : :obj:`str`, optional + euler_angles: list of float + Three numbers that represent the angles of rotations about the + defined axes. + static: bool, optional + If true the rotations are applied to a static frame. If not, to a + rotational. Defaults to true. + axes: str, optional A 3 character string specifying order of the axes. Defaults to 'xyz'. Examples @@ -230,7 +233,11 @@ def from_euler_angles(cls, euler_angles, static=True, axes='xyz'): @property def quaternion(self): - """Returns the 4 quaternion coefficients from the ``Rotation``. + """Returns the Quaternion from the ``Rotation``. + + Returns + ------- + :class:`Quaternion` Examples -------- @@ -240,12 +247,17 @@ def quaternion(self): >>> allclose(q1, q2, tol=1e-3) True """ - return quaternion_from_matrix(self.matrix) + from compas.geometry.primitives import Quaternion + return Quaternion(*quaternion_from_matrix(self.matrix)) @property def axis_and_angle(self): """Returns the axis and the angle of the ``Rotation``. + Returns + ------- + tuple: (:class:`Vector`, float) + Examples -------- >>> axis1 = normalize_vector([-0.043, -0.254, 0.617]) @@ -257,7 +269,9 @@ def axis_and_angle(self): >>> allclose([angle1], [angle2]) True """ - return axis_and_angle_from_matrix(self.matrix) + from compas.geometry.primitives import Vector + axis, angle = axis_and_angle_from_matrix(self.matrix) + return Vector(*axis), angle @property def axis_angle_vector(self): @@ -265,9 +279,7 @@ def axis_angle_vector(self): Returns ------- - :obj:`list` of :obj:`float` - Three numbers that represent the axis of rotation and angle of rotation - through the vector's magnitude. + :class:`Vector` Examples -------- @@ -278,7 +290,7 @@ def axis_angle_vector(self): True """ axis, angle = self.axis_and_angle - return scale_vector(axis, angle) + return axis.scaled(angle) def euler_angles(self, static=True, axes='xyz'): """Returns Euler angles from the ``Rotation`` according to specified @@ -286,16 +298,16 @@ def euler_angles(self, static=True, axes='xyz'): Parameters ---------- - static : :obj:`bool`, optional - If true the rotations are applied to a static frame. - If not, to a rotational. Defaults to True. - axes : :obj:`str`, optional - A 3 character string specifying the order of the axes. Defaults to 'xyz'. + static : bool, optional + If true the rotations are applied to a static frame. If not, to a + rotational. Defaults to True. + axes : str, optional + A 3 character string specifying the order of the axes. Defaults to + 'xyz'. Returns ------- - :obj:`list` of :obj:`float` - The 3 Euler angles. + list of float: The 3 Euler angles. Examples -------- @@ -311,8 +323,15 @@ def euler_angles(self, static=True, axes='xyz'): @property def basis_vectors(self): """Returns the basis vectors of the ``Rotation``. + + Returns + ------- + tuple: (:class:`Vector`, :class:`Vector`) + """ - return basis_vectors_from_matrix(self.matrix) + from compas.geometry.primitives import Vector + xaxis, yaxis = basis_vectors_from_matrix(self.matrix) + return Vector(*xaxis), Vector(*yaxis) # ============================================================================== diff --git a/src/compas/geometry/transformations/transformation.py b/src/compas/geometry/transformations/transformation.py index de6d419306b9..3da02996d96e 100644 --- a/src/compas/geometry/transformations/transformation.py +++ b/src/compas/geometry/transformations/transformation.py @@ -97,7 +97,7 @@ def copy(self): return cls.from_matrix(self.matrix) def __repr__(self): - s = "[[%s],\n" % ",".join([("%.4f" % n).rjust(10) for n in self.matrix[0]]) + s = "[[%s],\n" % ",".join([("%.4f" % n).rjust(10) for n in self.matrix[0]]) s += " [%s],\n" % ",".join([("%.4f" % n).rjust(10) for n in self.matrix[1]]) s += " [%s],\n" % ",".join([("%.4f" % n).rjust(10) for n in self.matrix[2]]) s += " [%s]]\n" % ",".join([("%.4f" % n).rjust(10) for n in self.matrix[3]]) @@ -155,7 +155,7 @@ def from_list(cls, numbers): @classmethod def from_frame(cls, frame): - """Computes a change of basis transformation from world XY to frame. + """Computes a transformation from world XY to frame. Parameters ---------- @@ -182,11 +182,11 @@ def from_frame(cls, frame): @classmethod def from_frame_to_frame(cls, frame_from, frame_to): - """Computes a change of basis transformation between two frames. + """Computes a transformation between two frames. - This transformation maps geometry from one Cartesian coordinate system - defined by "frame_from" to the other Cartesian coordinate system - defined by "frame_to". + This transformation allows to transform geometry from one Cartesian + coordinate system defined by "frame_from" to another Cartesian + coordinate system defined by "frame_to". Parameters ---------- @@ -216,6 +216,36 @@ def from_frame_to_frame(cls, frame_from, frame_to): return cls(multiply_matrices(T2.matrix, matrix_inverse(T1.matrix))) + @classmethod + def change_basis(cls, frame_from, frame_to): + """Computes a change of basis transformation between two frames. + + A basis change is essentially a remapping of geometry from one + coordinate system to another. + + Args: + frame_from (:class:`Frame`): a frame defining the original + Cartesian coordinate system + frame_to (:class:`Frame`): a frame defining the targeted + Cartesian coordinate system + + Example: + >>> from compas.geometry import Point, Frame + >>> f1 = Frame([2, 2, 2], [0.12, 0.58, 0.81], [-0.80, 0.53, -0.26]) + >>> f2 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) + >>> T = Transformation.change_basis(f1, f2) + >>> p_f1 = Point(1, 1, 1) # point in f1 + >>> p_f1.transformed(T) # point represented in f2 + Point(1.395, 0.955, 1.934) + >>> Frame.local_to_local_coords(f1, f2, p_f1) + Point(1.395, 0.955, 1.934) + """ + + T1 = cls.from_frame(frame_from) + T2 = cls.from_frame(frame_to) + + return cls(multiply_matrices(matrix_inverse(T2.matrix), T1.matrix)) + def inverse(self): """Returns the inverse transformation. diff --git a/src/compas/geometry/transformations/transformations.py b/src/compas/geometry/transformations/transformations.py index 46b3c3eb65c3..b819efb4611d 100644 --- a/src/compas/geometry/transformations/transformations.py +++ b/src/compas/geometry/transformations/transformations.py @@ -21,6 +21,7 @@ from compas.geometry.basic import multiply_matrix_vector from compas.geometry.basic import multiply_matrices from compas.geometry.basic import transpose_matrix +from compas.geometry.basic import norm_vector from compas.geometry.angles import angle_vectors @@ -37,14 +38,19 @@ from compas.geometry.transformations import matrix_from_axis_and_angle from compas.geometry.transformations import matrix_from_scale_factors -from compas.geometry.transformations import matrix_from_axis_and_angle +from compas.geometry.transformations import matrix_change_basis __all__ = [ 'local_axes', + 'orthonormalize_axes', 'transform_points', 'transform_vectors', + 'transform_frames', + + 'local_to_world_coords', + 'world_to_local_coords', 'translate_points', 'translate_points_xy', @@ -87,6 +93,42 @@ def local_axes(a, b, c): return normalize_vector(u), normalize_vector(v), normalize_vector(w) +def orthonormalize_axes(xaxis, yaxis): + """Corrects xaxis and yaxis to be unit vectors and orthonormal. + + Parameters + ---------- + xaxis: :class:`Vector` or list of float + yaxis: :class:`Vector` or list of float + + Returns + ------- + tuple: (xaxis, yaxis) + The corrected axes. + + Raises + ------ + ValueError: If xaxis and yaxis cannot span a plane. + + Examples + -------- + >>> xaxis = [1, 4, 5] + >>> yaxis = [1, 0, -2] + >>> xaxis, yaxis = orthonormalize_axes(xaxis, yaxis) + >>> allclose(xaxis, [0.1543, 0.6172, 0.7715], tol=0.001) + True + >>> allclose(yaxis, [0.6929, 0.4891, -0.5298], tol=0.001) + True + """ + xaxis = normalize_vector(xaxis) + yaxis = normalize_vector(yaxis) + zaxis = cross_vectors(xaxis, yaxis) + if not norm_vector(zaxis): + raise ValueError("Xaxis and yaxis cannot span a plane.") + yaxis = cross_vectors(normalize_vector(zaxis), xaxis) + return xaxis, yaxis + + def homogenize(vectors, w=1.0): """Homogenise a list of vectors. @@ -140,19 +182,171 @@ def dehomogenize(vectors): return [[x / w, y / w, z / w] if w else [x, y, z] for x, y, z, w in vectors] +def homogenize_and_flatten_frames(frames): + """Homogenize a list of frames and flatten the 3D list into a 2D list. + + Parameters + ---------- + frames: list of :class:`Frame` + + Returns + ------- + list of list of float + + Examples + -------- + >>> frames = [Frame((1, 1, 1), (0, 1, 0), (1, 0, 0))] + >>> homogenize_and_flatten_frames(frames) + [[1.0, 1.0, 1.0, 1.0], [0.0, 1.0, 0.0, 0.0], [1.0, -0.0, 0.0, 0.0]] + """ + def homogenize_frame(frame): + return homogenize([frame[0]], w=1.0) + homogenize([frame[1], frame[2]], w=0.0) + return [v for frame in frames for v in homogenize_frame(frame)] + + +def dehomogenize_and_unflatten_frames(points_and_vectors): + """Dehomogenize a list of vectors and unflatten the 2D list into a 3D list. + + Parameters + ---------- + points_and_vectors: list of list of float + Homogenized points and vectors. + + Returns + ------- + list of list of list of float + The frames. + + Examples + -------- + >>> points_and_vectors = [(1., 1., 1., 1.), (0., 1., 0., 0.), (1., 0., 0., 0.)] + >>> dehomogenize_and_unflatten_frames(points_and_vectors) + [[[1.0, 1.0, 1.0], [0.0, 1.0, 0.0], [1.0, 0.0, 0.0]]] + """ + frames = dehomogenize(points_and_vectors) + return [frames[i:i+3] for i in range(0, len(frames), 3)] + # ============================================================================== # transform # ============================================================================== def transform_points(points, T): + """Transform multiple points with one transformation matrix. + + Parameters + ---------- + points : list of :class:`Point` or list of list of float + A list of points to be transformed. + T : :class:`Transformation` or list of list of float + The transformation to apply. + + Examples + -------- + >>> points = [[1, 0, 0], [1, 2, 4], [4, 7, 1]] + >>> T = matrix_from_axis_and_angle([0, 2, 0], math.radians(45), point=[4, 5, 6]) + >>> points_transformed = transform_points(points, T) + """ return dehomogenize(multiply_matrices(homogenize(points, w=1.0), transpose_matrix(T))) def transform_vectors(vectors, T): + """Transform multiple vectors with one transformation matrix. + + Parameters + ---------- + vectors : list of :class:`Vector` or list of list of float + A list of vectors to be transformed. + T : :class:`Transformation` list of list of float + The transformation to apply. + + Examples + -------- + >>> vectors = [[1, 0, 0], [1, 2, 4], [4, 7, 1]] + >>> T = matrix_from_axis_and_angle([0, 2, 0], math.radians(45), point=[4, 5, 6]) + >>> vectors_transformed = transform_vectors(vectors, T) + """ return dehomogenize(multiply_matrices(homogenize(vectors, w=0.0), transpose_matrix(T))) +def transform_frames(frames, T): + """Transform multiple frames with one transformation matrix. + + Parameters + ---------- + frames : list of :class:`Frame` + A list of frames to be transformed. + T : :class:`Transformation` + The transformation to apply on the frames. + + Examples + -------- + >>> frames = [Frame([1, 0, 0], [1, 2, 4], [4, 7, 1]), Frame([0, 2, 0], [5, 2, 1], [0, 2, 1])] + >>> T = matrix_from_axis_and_angle([0, 2, 0], math.radians(45), point=[4, 5, 6]) + >>> transformed_frames = transform_frames(frames, T) + """ + points_and_vectors = homogenize_and_flatten_frames(frames) + return dehomogenize_and_unflatten_frames(multiply_matrices(points_and_vectors, transpose_matrix(T))) + + +def world_to_local_coords(frame, xyz): + """Convert global coordinates to local coordinates. + + Parameters + ---------- + frame : :class:`Frame` or [point, xaxis, yaxis] + The local coordinate system. + xyz : array-like + The global coordinates of the points to convert. + + Returns + ------- + list of list of float + The coordinates of the given points in the local coordinate system. + + + Examples + -------- + >>> import numpy as np + >>> f = Frame([0, 1, 0], [3, 4, 1], [1, 5, 9]) + >>> xyz = [Point(2, 3, 5)] + >>> Point(*world_to_local_coords(f, xyz)[0]) + Point(3.726, 4.088, 1.550) + """ + from compas.geometry.primitives import Frame + T = matrix_change_basis(Frame.worldXY(), frame) + return transform_points(xyz, T) + + +def local_to_world_coords(frame, xyz): + """Convert local coordinates to global coordinates. + + Parameters + ---------- + frame : :class:`Frame` or [point, xaxis, yaxis] + The local coordinate system. + xyz : list of `Points` or list of list of float + The global coordinates of the points to convert. + + Returns + ------- + list of list of float + The coordinates of the given points in the local coordinate system. + + + Examples + -------- + >>> import numpy as np + >>> f = Frame([0, 1, 0], [3, 4, 1], [1, 5, 9]) + >>> xyz = [Point(3.726, 4.088, 1.550)] + >>> Point(*local_to_world_coords(f, xyz)[0]) + Point(2.000, 3.000, 5.000) + """ + from compas.geometry.primitives import Frame + T = matrix_change_basis(frame, Frame.worldXY()) + return transform_points(xyz, T) + + # ============================================================================== # translate # ============================================================================== @@ -611,7 +805,7 @@ def project_point_plane(point, plane): >>> point = [3.0, 3.0, 3.0] >>> plane = ([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]) # the XY plane >>> project_point_plane(point, plane) - [3.0, 3.0, 3.0] + [3.0, 3.0, 0.0] """ base, normal = plane @@ -853,7 +1047,7 @@ def reflect_line_triangle(line, triangle, tol=1e-6): >>> triangle = [1.0, 0, 0], [-1.0, 0, 0], [0, 0, 1.0] >>> line = [-1, 1, 0], [-0.5, 0.5, 0] >>> reflect_line_triangle(line, triangle) - ([0.0, 0.0, 0], [1.0, 1.0, 0]) + ([0.0, 0.0, 0.0], [1.0, 1.0, 0.0]) """ x = intersection_line_triangle(line, triangle, tol=tol) @@ -907,32 +1101,29 @@ def orient_points(points, reference_plane, target_plane): Examples -------- - .. code-block:: python - - from compas.geometry import orient_points - from compas.geometry import intersection_segment_segment_xy - - refplane = ([0.57735, 0.57735, 0.57735], [1.0, 1.0, 1.0]) - tarplane = ([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]) - - points = [ - [0.288675, 0.288675, 1.1547], - [0.866025, 0.866025, 0.0], - [1.077350, 0.077350, 0.57735], - [0.077350, 1.077350, 0.57735] + >>> from compas.geometry import orient_points + >>> from compas.geometry import intersection_segment_segment_xy + >>> + >>> refplane = ([0.57735, 0.57735, 0.57735], [1.0, 1.0, 1.0]) + >>> tarplane = ([0.0, 0.0, 0.0], [0.0, 0.0, 1.0]) + >>> + >>> points = [\ + [0.288675, 0.288675, 1.1547],\ + [0.866025, 0.866025, 0.0],\ + [1.077350, 0.077350, 0.57735],\ + [0.077350, 1.077350, 0.57735]\ ] - - points = orient_points(points, refplane, tarplane) - - ab = points[0], points[1] - cd = points[2], points[3] - - point = intersection_segment_segment_xy(ab, cd) - - points = orient_points([point], tarplane, refplane) - - print(points[0]) - + >>> + >>> points = orient_points(points, refplane, tarplane) + >>> + >>> ab = points[0], points[1] + >>> cd = points[2], points[3] + >>> + >>> point = intersection_segment_segment_xy(ab, cd) + >>> + >>> points = orient_points([point], tarplane, refplane) + >>> Point(*points[0]) + Point(0.577, 0.577, 0.577) """ axis = cross_vectors(reference_plane[1], target_plane[1]) angle = angle_vectors(reference_plane[1], target_plane[1]) @@ -953,4 +1144,8 @@ def orient_points(points, reference_plane, target_plane): if __name__ == "__main__": - pass + import doctest + from compas.geometry import allclose + from compas.geometry import Point + from compas.geometry import Frame + doctest.testmod(globs=globals()) diff --git a/src/compas/geometry/transformations/transformations_numpy.py b/src/compas/geometry/transformations/transformations_numpy.py index 55747dfe705e..fdecce465453 100644 --- a/src/compas/geometry/transformations/transformations_numpy.py +++ b/src/compas/geometry/transformations/transformations_numpy.py @@ -5,9 +5,13 @@ from numpy import asarray from numpy import hstack from numpy import ones +from numpy import vectorize +from numpy import tile from scipy.linalg import solve +from compas.geometry.basic import cross_vectors + __all__ = [ 'transform_points_numpy', @@ -16,51 +20,86 @@ 'homogenize_numpy', 'dehomogenize_numpy', - 'local_coords_numpy', - 'global_coords_numpy', + 'homogenize_and_flatten_frames_numpy', + 'dehomogenize_and_unflatten_frames_numpy', + + 'world_to_local_coords_numpy', + 'local_to_world_coords_numpy', + ] def transform_points_numpy(points, T): + """Transform multiple points with one Transformation using numpy. + + Parameters + ---------- + points : list of :class:`Point` or list of list of float + A list of points to be transformed. + T : :class:`Transformation` or list of list of float + The transformation to apply. + + Examples + -------- + >>> points = [[1, 0, 0], [1, 2, 4], [4, 7, 1]] + >>> T = matrix_from_axis_and_angle([0, 2, 0], math.radians(45), point=[4, 5, 6]) + >>> points_transformed = transform_points_numpy(points, T) + """ T = asarray(T) points = homogenize_numpy(points, w=1.0) return dehomogenize_numpy(points.dot(T.T)) def transform_vectors_numpy(vectors, T): + """Transform multiple vectors with one Transformation using numpy. + + Parameters + ---------- + vectors : list of :class:`Vector` + A list of vectors to be transformed. + T : :class:`Transformation` + The transformation to apply. + + Examples + -------- + >>> vectors = [[1, 0, 0], [1, 2, 4], [4, 7, 1]] + >>> T = matrix_from_axis_and_angle([0, 2, 0], math.radians(45), point=[4, 5, 6]) + >>> vectors_transformed = transform_vectors_numpy(vectors, T) + """ T = asarray(T) vectors = homogenize_numpy(vectors, w=0.0) return dehomogenize_numpy(vectors.dot(T.T)) -# ============================================================================== -# helping helpers -# ============================================================================== - - -def homogenize_numpy(points, w=1.0): - points = asarray(points) - points = hstack((points, w * ones((points.shape[0], 1)))) - return points - +def transform_frames_numpy(frames, T): + """Transform multiple frames with one Transformation usig numpy. -def dehomogenize_numpy(points): - points = asarray(points) - return points[:, :-1] / points[:, -1].reshape((-1, 1)) + Parameters + ---------- + frames : list of :class:`Frame` + A list of frames to be transformed. + T : :class:`Transformation` + The transformation to apply on the frames. + + Examples + -------- + >>> frames = [Frame([1, 0, 0], [1, 2, 4], [4, 7, 1]), Frame([0, 2, 0], [5, 2, 1], [0, 2, 1])] + >>> T = matrix_from_axis_and_angle([0, 2, 0], math.radians(45), point=[4, 5, 6]) + >>> transformed_frames = transform_frames_numpy(frames, T) + """ + from numpy import asarray + T = asarray(T) + points_and_vectors = homogenize_and_flatten_frames_numpy(frames) + return dehomogenize_and_unflatten_frames_numpy(points_and_vectors.dot(T.T)) -# this should be defined somewhere else -# and should have a python equivalent -# there is an implementation available in frame -def local_coords_numpy(origin, uvw, xyz): +def world_to_local_coords_numpy(frame, xyz): """Convert global coordinates to local coordinates. Parameters ---------- - origin : array-like - The global (XYZ) coordinates of the origin of the local coordinate system. - uvw : array-like - The global coordinate difference vectors of the axes of the local coordinate system. + frame : :class:`Frame` or [point, xaxis, yaxis] + The local coordinate system. xyz : array-like The global coordinates of the points to convert. @@ -69,29 +108,33 @@ def local_coords_numpy(origin, uvw, xyz): array The coordinates of the given points in the local coordinate system. - Notes - ----- - ``origin`` and ``uvw`` together form the frame of local coordinates. - + Examples + -------- + >>> import numpy as np + >>> frame = Frame([0, 1, 0], [3, 4, 1], [1, 5, 9]) + >>> xyz = [Point(2, 3, 5)] + >>> rst = world_to_local_coords_numpy(frame, xyz) + >>> np.allclose(rst, [[3.726, 4.088, 1.550]], rtol=1e-3) + True """ + from numpy import asarray + from scipy.linalg import solve + + origin = frame[0] + uvw = [frame[1], frame[2], cross_vectors(frame[1], frame[2])] uvw = asarray(uvw).T xyz = asarray(xyz).T - asarray(origin).reshape((-1, 1)) rst = solve(uvw, xyz) return rst.T -# this should be defined somewhere else -# and should have a python equivalent -# there is an implementation available in frame -def global_coords_numpy(origin, uvw, rst): +def local_to_world_coords_numpy(frame, rst): """Convert local coordinates to global (world) coordinates. Parameters ---------- - origin : array-like - The origin of the local coordinate system. - uvw : array-like - The coordinate axes of the local coordinate system. + frame : :class:`Frame` or [point, xaxis, yaxis] + The local coordinate system. rst : array-like The coordinates of the points wrt the local coordinate system. @@ -104,17 +147,139 @@ def global_coords_numpy(origin, uvw, rst): ----- ``origin`` and ``uvw`` together form the frame of local coordinates. + Examples + -------- + >>> frame = Frame([0, 1, 0], [3, 4, 1], [1, 5, 9]) + >>> rst = [Point(3.726, 4.088, 1.550)] + >>> xyz = local_to_world_coords_numpy(frame, rst) + >>> numpy.allclose(xyz, [[2.000, 3.000, 5.000]], rtol=1e-3) + True """ + from numpy import asarray + + origin = frame[0] + uvw = [frame[1], frame[2], cross_vectors(frame[1], frame[2])] + uvw = asarray(uvw).T rst = asarray(rst).T xyz = uvw.dot(rst) + asarray(origin).reshape((-1, 1)) return xyz.T +# ============================================================================== +# helping helpers +# ============================================================================== + + +def homogenize_numpy(points, w=1.0): + """Dehomogenizes points or vectors. + + Parameters + ---------- + points: list of :class:`Points` or list of :class:`Vectors` + + Returns + ------- + :class:`numpy.ndarray` + + Examples + -------- + >>> points = [[1, 1, 1], [0, 1, 0], [1, 0, 0]] + >>> res = homogenize_numpy(points, w=1.0) + >>> numpy.allclose(res, [[1.0, 1.0, 1.0, 1.0], [0.0, 1.0, 0.0, 1.0], [1.0, -0.0, 0.0, 1.0]]) + True + """ + points = asarray(points) + points = hstack((points, w * ones((points.shape[0], 1)))) + return points + + +def dehomogenize_numpy(points): + """Dehomogenizes points or vectors. + + Parameters + ---------- + points: list of :class:`Points` or list of :class:`Vectors` + + Returns + ------- + :class:`numpy.ndarray` + + Examples + -------- + >>> points = [[1, 1, 1, 0], [0, 1, 0, 0], [1, 0, 0, 0]] + >>> res = dehomogenize_numpy(points) + >>> numpy.allclose(res, [[1.0, 1.0, 1.0], [0.0, 1.0, 0.0], [1.0, -0.0, 0.0]]) + True + """ + def func(a): + return a if a else 1. + func = vectorize(func) + + points = asarray(points) + return points[:, :-1] / func(points[:, -1]).reshape((-1, 1)) + + +def homogenize_and_flatten_frames_numpy(frames): + """Homogenize a list of frames and flatten the 3D list into a 2D list using numpy. + + The frame consists of a point and 2 orthonormal vectors. + + Parameters + ---------- + frames: list of :class:`Frame` + + Returns + ------- + :class:`numpy.ndarray` + An array of points and vectors. + + Examples + -------- + >>> frames = [Frame((1, 1, 1), (0, 1, 0), (1, 0, 0))] + >>> res = homogenize_and_flatten_frames_numpy(frames) + >>> numpy.allclose(res, [[1.0, 1.0, 1.0, 1.0], [0.0, 1.0, 0.0, 0.0], [1.0, -0.0, 0.0, 0.0]]) + True + """ + n = len(frames) + frames = asarray(frames).reshape(n * 3, 3) + extend = tile(asarray([1, 0, 0]).reshape(3, 1), (n, 1)) + return hstack((frames, extend)) + + +def dehomogenize_and_unflatten_frames_numpy(points_and_vectors): + """Dehomogenize a list of vectors and unflatten the 2D list into a 3D list. + + Parameters + ---------- + points_and_vectors: list of list of float + Homogenized points and vectors. + + Returns + ------- + :class:`numpy.ndarray` + The frames. + + Examples + -------- + >>> points_and_vectors = [(1., 1., 1., 1.), (0., 1., 0., 0.), (1., 0., 0., 0.)] + >>> res = dehomogenize_and_unflatten_frames_numpy(points_and_vectors) + >>> numpy.allclose(res, [[1.0, 1.0, 1.0], [0.0, 1.0, 0.0], [1.0, 0.0, 0.0]]) + True + """ + frames = dehomogenize_numpy(points_and_vectors) + return frames.reshape((int(frames.shape[0]/3.), 3, 3)) + + # ============================================================================== # Main # ============================================================================== if __name__ == "__main__": - - pass + import doctest + import numpy + import math + from compas.geometry import Frame + from compas.geometry import Point + from compas.geometry import matrix_from_axis_and_angle + doctest.testmod(globs=globals())