Source code for Muscat.LinAlg.Transform
# -*- coding: utf-8 -*-
#
# This file is subject to the terms and conditions defined in
# file 'LICENSE.txt', which is part of this source code package.
#
from __future__ import annotations
from typing import Optional
import numpy as np
from Muscat.Types import MuscatFloat, MuscatIndex, ArrayLike
[docs]class Transform:
def __init__(self, offset: Optional[ArrayLike] = None, first: Optional[ArrayLike] = None, second: Optional[ArrayLike] = None):
"""Class to do a linear transformation of coordinates
Parameters
----------
offset : Optional[ArrayLike], optional
offset vector of size 3, by default None
first : Optional[ArrayLike], optional
first direction vector , by default None
second : Optional[ArrayLike], optional
second vector to construct the coordinate system, by default None
"""
super().__init__()
self.offset = np.asarray([0.0, 0.0, 0.0], dtype=MuscatFloat)
self.RMatrix = np.asarray([[1.0, 0, 0], [0, 1, 0], [0, 0, 1]], dtype=MuscatFloat)
self.keepOrthogonal = True
self.keepNormalized = True
# offset off the new origin with respect to the old
if offset is not None:
self.SetOffset(offset)
if first is not None:
self.SetFirst(first)
if second is not None:
self.SetSecond(second)
[docs] def SetAsRotationAroundAxis(self, vector: ArrayLike, angle: MuscatFloat, center: Optional[ArrayLike] = None) -> None:
"""Setter method for setting the transform as a rotation around a vector with a given angle
the transformation matrix and offset are computed from the center and the angle
Parameters
----------
vector : ArrayLike
must be 1D of size 3, vector around which the rotation is performed
angle : MuscatFloat
angle (in rad) for the rotation
center : Optional[ArrayLike], optional
center point for the rotation of size 3, by default None
"""
# Normalizing vector
vector = vector / np.linalg.norm(vector)
RMatrix = np.asarray(
[[
np.cos(angle) + vector[0] ** 2 * (1 - np.cos(angle)),
vector[0] * vector[1] * (1 - np.cos(angle)) - vector[2] * np.sin(angle),
vector[0] * vector[2] * (1 - np.cos(angle)) + vector[1] * np.sin(angle),
],[
vector[0] * vector[1] * (1 - np.cos(angle)) + vector[2] * np.sin(angle),
np.cos(angle) + vector[1] ** 2 * (1 - np.cos(angle)),
vector[1] * vector[2] * (1 - np.cos(angle)) - vector[0] * np.sin(angle),
],[
vector[0] * vector[2] * (1 - np.cos(angle)) - vector[1] * np.sin(angle),
vector[1] * vector[2] * (1 - np.cos(angle)) + vector[0] * np.sin(angle),
np.cos(angle) + vector[2] ** 2 * (1 - np.cos(angle)),
]],
dtype=MuscatFloat,
)
# Offset is computed according to simple development of rotation around center
self.offset = center - RMatrix.T @ center
# Rotation matrix given axis and angle: this bypasses the SetFirst, SetSecond method since it does not need checks
self.RMatrix = RMatrix
[docs] def GetDirection(self, i: MuscatIndex, pos: Optional[ArrayLike] = None, direction: Optional[ArrayLike] = None) -> np.ndarray:
"""Return the direction of the i component of the coordinate system.
for linear transformation pos is not used
Parameters
----------
i : MuscatIndex
must be in [0,2]
pos : Optional[ArrayLike], optional
_description_, by default None
direction : Optional[ArrayLike], optional
_description_, by default None
Returns
-------
np.ndarray
the i vector of the base
"""
return self.RMatrix[i, :]
[docs] def SetOffset(self, data: ArrayLike) -> None:
"""Set the origin of the new coordinate system
Parameters
----------
data : ArrayLike
offset
"""
self.offset = np.asarray(data, dtype=MuscatFloat)
[docs] def SetFirst(self, data: ArrayLike) -> None:
"""define the x coordinate (direction) with respect to the new origin
Parameters
----------
data : ArrayLike
the first direction of the coordinate system
"""
first = np.array(data, dtype=MuscatFloat)
if self.keepNormalized:
first /= np.linalg.norm(first)
self.RMatrix[0, :] = first
self.SetSecond()
self.SetThird()
[docs] def SetSecond(self, data: Optional[ArrayLike] = None) -> None:
"""Define the y coordinate (direction) with respect to the new origin
# the z direction is calculated with a cross product
Parameters
----------
data : ArrayLike, optional
if non the second vector of the internal rotation matrix is used , by default None
Raises
------
ValueError
RuntimeError if the incoming vector is colinear to the first vector
"""
first = self.RMatrix[0, :]
if data is None:
second = self.RMatrix[1, :] - first * np.dot(first, self.RMatrix[1, :]) / np.dot(first, first)
# if norm is zero compute a vector not colinear to first and we restart
if np.linalg.norm(second) == 0:
second = first * 1
for i, v in enumerate(second):
if v == 0:
second[i] += 1
break
else:
second[0] += 1
return self.SetSecond(second)
else:
second = np.array(data, dtype=MuscatFloat)
if self.keepOrthogonal:
second -= first * np.dot(first, second) / np.dot(first, first)
second_norm = np.linalg.norm(second)
if second_norm == 0: # pragma: no cover
raise ValueError("cant set second to " + str(data) + " This vector ir colinear to first :" + str(first))
if self.keepNormalized:
second /= second_norm
self.RMatrix[1, :] = second
self.SetThird()
[docs] def SetThird(self, data: Optional[ArrayLike] = None) -> None:
"""Define the z coordinate (direction) with respect to the new origin
# the z direction is calculated with a cross product of x, y
Parameters
----------
data : ArrayLike, optional
_description_, by default None
Raises
------
ValueError
if the incoming vector is colinear to x, y
"""
first = self.RMatrix[0, :]
second = self.RMatrix[1, :]
if data is None:
third = np.cross(first, second)
else:
third = np.array(data, dtype=MuscatFloat)
if self.keepOrthogonal:
third -= first * np.dot(first, third) / np.dot(first, first)
third -= second * np.dot(second, third) / np.dot(second, second)
third_norm = np.linalg.norm(third)
if third_norm == 0: # pragma: no cover
raise Exception("cant set third to " + str(data) + " This vector ir colinear to first and/or second :" + str(first) + " " + str(second))
if self.keepNormalized:
third /= third_norm
self.RMatrix[2, :] = third
[docs] def SetOpUsingThird(self, third: ArrayLike) -> None:
self.SetFirst(third)
self.SetOperator(op=np.roll(self.RMatrix, -1, axis=0))
[docs] def SetOperator(self, first: Optional[ArrayLike] = None, second: Optional[ArrayLike] = None, third: Optional[ArrayLike] = None, op=None) -> None:
if op is None:
self.SetFirst(first)
self.SetSecond(second)
self.SetThird(third)
else:
if (first is not None) or (second is not None) or (third is not None): # pragma: no cover
raise (Exception("Cant define operator and direction vector at the same time"))
self.SetOperator(first=op[0, :], second=op[1, :], third=op[2, :])
[docs] def ApplyTransform(self, point: ArrayLike) -> np.ndarray:
"""p = M*(point-self.offset)
Parameters
----------
point : ArrayLike
List of points to apply the transformation
Returns
-------
np.ndarray
the position of the point on the new reference system
"""
point = np.asarray(point, dtype=MuscatFloat)
op = self.RMatrix
return self.__ApplyOpForEveryLine(op, point - self.offset)
[docs] def ApplyInvTransform(self, point: ArrayLike) -> np.ndarray:
"""we apply inverse of the transformation
#p = M^-1*point+self.offset
Parameters
----------
point : ArrayLike
List of points to apply the transformation
Returns
-------
np.ndarray
the position of the point on the new reference system
"""
point = np.asarray(point)
if self.keepNormalized == False or self.keepOrthogonal == False:
op = np.linalg.inv(self.RMatrix)
else:
op = self.RMatrix.T
return self.__ApplyOpForEveryLine(op, point) + self.offset
[docs] def ApplyTransformDirection(self, point: ArrayLike) -> np.ndarray:
"""Apply only the direction transformation (not the offset)
p = M*(point)
Parameters
----------
point : ArrayLike
List of points to apply the transformation
Returns
-------
np.ndarray
the position of the point on the new reference system
"""
point = np.asarray(point)
op = self.RMatrix
return self.__ApplyOpForEveryLine(op, point)
[docs] def ApplyInvTransformDirection(self, point: ArrayLike) -> np.ndarray:
"""we apply inverse of the transformation (only the direction, no offset)
p = M^-1*(point)
Parameters
----------
point : ArrayLike
List of points to apply the transformation
Returns
-------
np.ndarray
the position of the point on the new reference system
"""
point = np.asarray(point, dtype=MuscatFloat)
if self.keepNormalized == False or self.keepOrthogonal == False:
op = np.linalg.inv(self.RMatrix)
else:
op = self.RMatrix.T
return self.__ApplyOpForEveryLine(op, point)
[docs] def ApplyTransformTensor(self, tensor: ArrayLike) -> np.ndarray:
"""Apply rotation on a tensor
Parameters
----------
tensor : ArrayLike
np array of size (3,3)
Returns
-------
np.ndarray
the tensor on the new base
"""
tensor = np.asarray(tensor)
op = self.RMatrix
return np.dot(op, np.dot(tensor, op.T))
[docs] def ApplyInvTransformTensor(self, tensor: ArrayLike) -> np.ndarray:
"""Apply the inverse of the transformation on a tensor
Parameters
----------
tensor : ArrayLike
np array of size (3,3)
Returns
-------
np.ndarray
the tensor on the (old) base
"""
#
if self.keepNormalized == False or self.keepOrthogonal == False:
op = np.linalg.inv(self.RMatrix)
else:
op = self.RMatrix.T
return np.dot(op, np.dot(tensor, op.T))
def __ApplyOpForEveryLine(self, op: ArrayLike, points: np.ndarray) -> np.ndarray:
"""Internal function"""
if len(points.shape) == 2:
return np.dot(op, points.T).T
else:
return np.dot(op, points)
[docs] def GetOrthoNormalBase(self) -> Transform:
"""Return a new transform but with orthogonal base
Returns
-------
Transform
_description_
"""
return Transform(self.offset, self.RMatrix[0, :], self.RMatrix[1, :])
def __str__(self):
res = "Transform : \n"
res += " keepNormalized : " + str(self.keepNormalized) + "\n"
res += " keepOrthogonal : " + str(self.keepOrthogonal) + "\n"
res += " offset : " + str(self.offset) + "\n"
res += " first : " + str(self.RMatrix[0, :]) + "\n"
res += " second : " + str(self.RMatrix[1, :]) + "\n"
res += " third : " + str(self.RMatrix[2, :]) + "\n"
return res
[docs]class Transform2D:
"""This class is a wrapper around the Transform class to work on points in 2D.
Please refer to the documentation of Transform for all the functions
"""
def __init__(self, offset: Optional[ArrayLike] = None, first: Optional[ArrayLike] = None, second: Optional[ArrayLike] = None):
super().__init__()
offset = self.EnsureIn3D(offset)
first = self.EnsureIn3D(first)
second = self.EnsureIn3D(second)
self.transform3D = Transform(offset=offset, first=first, second=second)
@property
def keepOrthogonal(self):
return self.transform3D.keepOrthogonal
@property
def keepNormalized(self):
return self.transform3D.keepNormalized
@keepOrthogonal.setter
def keepOrthogonal(self, value):
self.transform3D.keepOrthogonal = value
@keepNormalized.setter
def keepNormalized(self, value):
self.transform3D.keepNormalized = value
[docs] def GetDirection(self, i: MuscatIndex, pos: Optional[np.ndarray] = None, direction: Optional[np.ndarray] = None):
return self.EnsureIn2D(self.transform3D.GetDirection(pos, direction))
[docs] def ApplyTransform(self, points: np.ndarray):
return self.EnsureIn2D(self.transform3D.ApplyTransform(self.EnsureIn3D(points)))
[docs] def ApplyInvTransform(self, points: np.ndarray):
return self.EnsureIn2D(self.transform3D.ApplyInvTransform(self.EnsureIn3D(points)))
[docs] def ApplyTransformDirection(self, direction: np.ndarray):
return self.EnsureIn2D(self.transform3D.ApplyTransformDirection(self.EnsureIn3D(direction)))
[docs] def ApplyInvTransformDirection(self, point: np.ndarray):
return self.EnsureIn2D(self.transform3D.ApplyInvTransformDirection(self.EnsureIn3D(point)))
[docs] def GetOrthoNormalBase(self):
return Transform2D(self.transform3D.offset, self.transform3D.RMatrix[0, :], self.transform3D.RMatrix[1, :])
[docs] def EnsureIn2D(self, data):
if data is None:
return None
data = np.asarray(data)
if len(data.shape) == 1:
return data[:2]
else:
return np.ascontiguousarray(data[:, :2])
[docs] def EnsureIn3D(self, data: Optional[np.ndarray]) -> np.ndarray:
if data is None:
return None
data = np.asarray(data)
if len(data.shape) == 1:
if data.shape[0] == 3:
return data
res = np.empty((3), dtype=data.dtype)
res[: data.shape[0]] = data
res[data.shape[0] :] = 0
return res
else:
if data.shape[1] == 3:
return data
res = np.empty((data.shape[0], 3), dtype=data.dtype)
res[:, : data.shape[1]] = data
res[:, data.shape[1] :] = 0
return res
[docs]def CheckIntegrity(GUI: bool = False):
orient = Transform()
orient.SetOpUsingThird([5, 2, 1])
orient.SetOperator(op=orient.RMatrix)
orient = Transform(offset=[1, 1, 1], first=[0, 1, 0])
orient = Transform(offset=[1, 1, 1], first=[1, 0, 0], second=[0, 1, 0])
print(orient.GetDirection(0))
# test with a list of points
orient.ApplyTransform([[0, 1, 2], [3, 4, 5]])
print(orient.GetOrthoNormalBase())
def CheckOperations(orient, p, v, t):
pt = orient.ApplyTransform(p)
pr = orient.ApplyInvTransform(pt)
if np.linalg.norm(p - pr) > 1e-14: # pragma: no cover
print(orient)
print("Error ", np.linalg.norm(p - pr))
print("diff ", (p - pr))
print("p ", p)
print("pt ", pt)
print("pr ", pr)
raise Exception("Error in the position transform")
vt = orient.ApplyTransformDirection(v)
vr = orient.ApplyInvTransformDirection(vt)
if np.linalg.norm(v - vr) > 1e-14: # pragma: no cover
print(orient)
print("Error ", np.linalg.norm(v - vr))
print("diff ", (v - vr))
print("v ", v)
print("vt ", vt)
print("vr ", vr)
raise Exception("Error in the direction transform")
tt = orient.ApplyTransformTensor(t)
tr = orient.ApplyInvTransformTensor(tt)
if np.linalg.norm(t - tr) > 1e-14: # pragma: no cover
print(orient)
print("Error ", np.linalg.norm(t - tr))
print("diff ", (t - tr))
print("t ", t)
print("tt ", tt)
print("tr ", tr)
raise Exception("Error in the tensor transform")
for nor in (True, False):
for ort in (True, False):
orient = Transform()
orient.keepNormalized = nor
orient.keepOrthogonal = ort
orient.SetFirst([1.0, 0.0, 0.0])
orient.SetSecond([1.0, 1.0, 0.0])
orient.SetThird([1.0, 1.0, 1.0])
p = [1.0, 3.0, 3.0]
v = [1.0, 4.0, 5.0]
t = np.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]])
CheckOperations(orient, p, v, t)
orient.SetAsRotationAroundAxis(vector=np.array([0.0, 0.0, 1.0]), angle=np.pi / 4, center=np.array([0.0, 0.0, 0]))
points = np.stack((np.linspace(0, 1, 10), np.linspace(0, 1, 10), np.zeros(10)), axis=-1)
np.testing.assert_allclose(orient.ApplyTransform(points)[:, 1], np.linspace(0, np.sqrt(2), 10))
orient = Transform2D(offset=[1, 1], first=[1, 0], second=[1, 1])
assert orient.EnsureIn2D(None) == None
np.testing.assert_equal(orient.EnsureIn2D([0, 1, 2]), [0, 1])
assert orient.EnsureIn3D(None) == None
np.testing.assert_equal(orient.EnsureIn3D([[0, 1, 2]]), [[0, 1, 2]])
orient.keepOrthogonal = True
assert orient.keepOrthogonal == True
orient.keepNormalized = True
assert orient.keepNormalized == True
orient.SetOffset([1, 1])
orient.SetOffset([1, 1, 0])
orient.SetFirst([1, 0])
orient.SetFirst([1, 0, 0])
print(orient.GetDirection(0))
# test with a list of points
data = np.array([[0, 1], [3, 4]], dtype=MuscatFloat)
data_new = orient.ApplyTransform(data)
data_new_neo = orient.ApplyInvTransform(data_new)
np.testing.assert_allclose(data, data_new_neo)
dir_data = np.array([[0, 1]], dtype=MuscatFloat)
dir_data_new = orient.ApplyTransformDirection(dir_data)
dir_data_new_neo = orient.ApplyInvTransformDirection(dir_data_new)
np.testing.assert_allclose(dir_data, dir_data_new_neo)
print(orient.GetOrthoNormalBase())
print(orient)
return "ok"
if __name__ == "__main__":
print((CheckIntegrity(GUI=True))) # pragma: no cover