ModuleGeometry

  1"""
  2Contains a bunch of useful function for geometric transformations and measurements.
  3Usually these don't need to be called by users of ART, but they may be useful.
  4
  5Some general conventions:
  6- As much as possible, avoid having lists of points or vectors transiting between functions.
  7    Instead, use the Vector and Point classes defined at the end of this file.
  8- Functions operating on Rays should preferentially operate on lists of Rays, not individual Rays.
  9    The reason for that is that it's fairly rare to manipluate a single Ray, and it's easier to
 10    just put it in a single-element list and call the function that way.
 11
 12Created in 2019
 13
 14@author: Anthony Guillaume + Stefan Haessler + André Kalouguine
 15"""
 16# %% Modules
 17import numpy as np
 18import quaternion
 19from functools import singledispatch
 20import logging
 21import math
 22
 23logger = logging.getLogger(__name__)
 24
 25# %% Points and Vectors classes
 26class Vector(np.ndarray):
 27    def __new__(cls, input_array):
 28        input_array = np.asarray(input_array)
 29        if input_array.ndim > 1:
 30            return VectorArray(input_array)
 31        obj = input_array.view(cls)  # Ensure we are viewing as `Vector`
 32        return obj
 33    @property
 34    def norm(self):
 35        return np.linalg.norm(self)
 36    def normalized(self):
 37        return self / self.norm
 38    def __add__(self, other):
 39        if isinstance(other, Point):
 40            return Point(super().__add__(other))
 41        return Vector(super().__add__(other))
 42    def __sub__(self, other):
 43        if isinstance(other, Point):
 44            return Point(super().__sub__(other))
 45        return Vector(super().__sub__(other))
 46    def translate(self, vector):
 47        return self
 48    def rotate(self,q):
 49        return Vector((q*np.quaternion(0,*self)*q.conj()).imag)
 50    def from_basis(self, r0, r, q):
 51        return self.rotate(q)
 52    def to_basis(self, r0, r, q):
 53        return self.rotate(q.conj())
 54    def _add_dimension(self, value=0):
 55        return Vector(np.concatenate((self, [value])))
 56    def __hash__(self) -> int:
 57        vector_tuple = tuple(self.reshape(1, -1)[0])
 58        return hash(vector_tuple)
 59class VectorArray(np.ndarray):
 60    def __new__(cls, input_array):
 61        input_array = np.asarray(input_array)
 62        if input_array.ndim == 1:
 63            return Vector(input_array)
 64        obj = input_array.view(cls)  # Ensure we are viewing as `Vector`
 65        return obj
 66    @property
 67    def norm(self):
 68        return np.linalg.norm(self, axis=1)
 69    def __getitem__(self, index):
 70        return Vector(super().__getitem__(index))
 71    def normalized(self):
 72        return self / self.norm[:, np.newaxis]
 73    def __add__(self, other):
 74        if isinstance(other, Point):
 75            return PointArray(super().__add__(other))
 76        return VectorArray(super().__add__(other))
 77    def __sub__(self, other):
 78        if isinstance(other, Point):
 79            return PointArray(super().__sub__(other))
 80        return VectorArray(super().__sub__(other))
 81    def translate(self, vector):
 82        return self
 83    def rotate(self,q):
 84        return VectorArray(quaternion.rotate_vectors(q, self))
 85    def from_basis(self, r0, r, q):
 86        return self.rotate(q)
 87    def to_basis(self, r0, r, q):
 88        return self.rotate(q.conj())
 89    def _add_dimension(self, values=0):
 90        if values == 0:
 91            values = np.zeros((self.shape[0], 1))
 92        if not isinstance(values, np.ndarray):
 93            values = np.array(values)
 94        return VectorArray(np.concatenate((self, values), axis=1))
 95    
 96class Point(np.ndarray):
 97    def __new__(cls, input_array):
 98        input_array = np.asarray(input_array)
 99        if input_array.ndim > 1:
100            return PointArray(input_array)
101        obj = input_array.view(cls)  # Ensure we are viewing as `Point`
102        return obj
103    def __add__(self, other):
104        return Point(super().__add__(other))
105    def __sub__(self, other):
106        return Vector(super().__sub__(other))
107    def translate(self, vector):
108        return Point(super()._add__(vector))
109    def rotate(self,q):
110        return (self-Origin).rotate(q)
111    def from_basis(self, r0, r, q):
112        return Point([0,0,0]) + r0 + Vector(self-r0).rotate(q) + r
113    def to_basis(self, r0, r, q):
114        return Point([0,0,0]) + r0 + Vector(self-r0-r).rotate(q.conj())
115    def _add_dimension(self, value=0):
116        return Point(np.concatenate((self, [value])))
117    def __hash__(self) -> int:
118        point_tuple = tuple(self.reshape(1, -1)[0])
119        return hash(point_tuple)
120
121class PointArray(np.ndarray):
122    def __new__(cls, input_array):
123        input_array = np.asarray(input_array)
124        if input_array.ndim == 1:
125            return Point(input_array)
126        obj = input_array.view(cls)  # Ensure we are viewing as `Point`
127        return obj
128    def __getitem__(self, index):
129        return Point(super().__getitem__(index))
130    def __add__(self, other):
131        return PointArray(super().__add__(other))
132    def __sub__(self, other):
133        return VectorArray(super().__sub__(other))
134    def translate(self, vector):
135        return PointArray(super().__add__(vector))
136    def rotate(self,q):
137        return PointArray(quaternion.rotate_vectors(q, self))
138    def from_basis(self, r0, r, q):
139        return Point([0,0,0]) + r0 + VectorArray(self-r0).rotate(q) + r
140        #return PointArray([Point([0,0,0]) + r0 + (p-r0).rotate(q) + r for p in self])
141    def to_basis(self, r0, r, q):
142        return Point([0,0,0]) + r0 + VectorArray(self-r0-r).rotate(q.conj())
143        #return PointArray([Point([0,0,0]) + r0 + (p-r0-r).rotate(q.conj()) for p in self])
144    def _add_dimension(self, values=0):
145        if values == 0:
146            values = np.zeros((self.shape[0], 1))
147        if not isinstance(values, np.ndarray):
148            values = np.array(values)
149        return PointArray(np.concatenate((self, values), axis=1))
150    
151Origin = Point([0,0,0])
152# %% More traditional vector operations that don't belong in the classes
153def Normalize(vector):
154    """
155    Normalize Vector.
156    Obsolete, use use the mgeo.Vector class instead as it has a `normalize` method.
157    """
158    return vector / np.linalg.norm(vector)
159
160def VectorPerpendicular(vector):
161    """
162    Find a perpendicular 3D vector in some arbitrary direction
163    Undefined behavior, use with caution. There is no unique perpendicular vector to a 3D vector.
164    """
165    logger.warning("VectorPerpendicular is undefined behavior. There is no unique perpendicular vector to a 3D vector.")
166    if abs(vector[0]) < 1e-15:
167        return Vector([1, 0, 0])
168    if abs(vector[1]) < 1e-15:
169        return Vector([0, 1, 0])
170    if abs(vector[2]) < 1e-15:
171        return Vector([0, 0, 1])
172
173    # set arbitrarily a = b =1
174    return Vector([1, 1, -1.0 * (vector[0] + vector[1]) / vector[2]]).normalized()
175
176def AngleBetweenTwoVectors(U, V):
177    """
178    Return the angle in radians between the vectors U and V ; formula from W.Kahan
179    Value in radians between 0 and pi.
180    """
181    u = np.linalg.norm(U)
182    v = np.linalg.norm(V)
183    return 2 * np.arctan2(np.linalg.norm(U * v - V * u), np.linalg.norm(U * v + V * u))
184
185def SymmetricalVector(V, SymmetryAxis):
186    """
187    Return the symmetrical vector to V
188    """
189    q = QRotationAroundAxis(SymmetryAxis, np.pi)
190    return V.rotate(q)
191
192def normal_add(N1, N2):
193    """
194    Simple function that takes in two normal vectors of a deformation and calculates
195    the total normal vector if the two deformations were individually applied.
196    Be very careful, this *only* works when the surface is z = f(x,y) and when 
197    the deformation is small.
198    Might be made obsolete when shifting to a modernised deformation system.
199    """
200    normal1 = N1.normalized()
201    normal2 = N2.normalized()
202    grad1 = -normal1[:2] / normal1[2]
203    grad2 = -normal2[:2] / normal2[2]
204    grad = grad1 + grad2
205    total_normal = np.append(-grad, 1)
206    return Vector(total_normal).normalized()
207
208# %% Intersection finding
209def IntersectionLinePlane(A, u, P, n):
210    """
211    Return the intersection point between a line and a plane.
212    A is a point of the line, u a vector of the line ; P is a point of the plane, n a normal vector
213    Line's equation : OM = u*t + OA , t a real
214    Plane's equation : n.OP - n.OM = 0
215    """
216    t = np.dot(n, -A + P) / np.dot(u, n)
217    I = u * t + A
218    return I
219
220def IntersectionRayListZPlane(RayList, Z=np.array([0])):
221    """
222    Return the intersection of a list of rays with a different planes with equiations z = Z[i]
223    Basically, by default it returns the intersection of the rays with the Z=0 plane but you can 
224    give it a few values of Z and it should be faster than calling it multiple times.
225    This should let us quickly find the optimal position of the detector as well as trace the caustics.
226    If a ray does not intersect the plane... it should replace that point with a NaN. 
227    """
228    Positions = np.vstack([i.point for i in RayList])
229    Vectors = np.vstack([i.vector for i in RayList])
230    non_zero = Vectors[:,2] != 0
231    Positions = Positions[non_zero]
232    Vectors = Vectors[non_zero]
233    Z = Z[:, np.newaxis]
234    A = Positions[:,2]-Z
235    B = -Vectors[:,2]
236    #times = (Positions[:,2]-Z)/Vectors[:,2]
237    #return A,B
238    with np.errstate(divide='ignore', invalid='ignore'):
239        times = np.divide(A, B, where=(B != 0), out=np.full_like(A, np.nan))
240        #times[times < 0] = np.nan  # Set negative results to NaN
241    #return times
242    #positive_times = times >= 0
243    intersect_positions = Positions[:, :2] + times[:, :, np.newaxis] * Vectors[:, :2]
244    result = []
245    for i in range(Z.shape[0]):
246        # For each plane, we find the intersection points
247        #valid_intersections = intersect_positions[i][positive_times[i]]
248        valid_intersections = intersect_positions[i]
249        result.append(PointArray(valid_intersections))
250    return result
251
252
253# %% Geometrical utilities for plotting
254def SpiralVogel(NbPoint, Radius):
255    """
256    Return a NbPoint x 2 matrix of 2D points representative of Vogel's spiral with radius Radius
257    Careful, contrary to most of the code, this is *not* in the 
258    ARTcore.Vector or ARTcore.Point format. It is a simple numpy array. 
259    The reason is that this is a utility function that can be used both to define directions
260    and to generate grids of points.
261    """
262    GoldenAngle = np.pi * (3 - np.sqrt(5))
263    r = np.sqrt(np.arange(NbPoint) / NbPoint) * Radius
264
265    theta = GoldenAngle * np.arange(NbPoint)
266
267    Matrix = np.zeros((NbPoint, 2))
268    Matrix[:, 0] = np.cos(theta)
269    Matrix[:, 1] = np.sin(theta)
270    Matrix = Matrix * r.reshape((NbPoint, 1))
271
272    return Matrix
273
274def find_hull(points):
275    """
276    Find the convex hull of a set of points using a greedy algorithm.
277    This is used to create a polygon that encloses the points.
278    """
279    # start from leftmost point
280    current_point = min(range(len(points)), key=lambda i: points[i][0])
281    # initialize hull with current point
282    hull = [current_point]
283    # initialize list of linked points
284    linked = []
285    # continue until all points have been linked
286    while len(linked) < len(points) - 1:
287        # initialize minimum distance and closest point
288        min_distance = math.inf
289        closest_point = None
290        # find closest unlinked point to current point
291        for i, point in enumerate(points):
292            if i not in linked:
293                distance = math.dist(points[current_point], point)
294                if distance < min_distance:
295                    min_distance = distance
296                    closest_point = i
297        # add closest point to hull and linked list
298        hull.append(closest_point)
299        linked.append(closest_point)
300        # update current point
301        current_point = closest_point
302    # add link between last point and first point
303    hull.append(hull[0])
304    # convert hull to a list of pairs of indices
305    indices = [[hull[i], hull[i + 1]] for i in range(len(hull) - 1)]
306    return indices
307
308
309# %% Solvers and utilities for solving equations
310def SolverQuadratic(a, b, c):
311    """
312    Solve the quadratic equation a*x^2 + b*x +c = 0 ; keep only real solutions
313    """
314    Solution = np.roots([a, b, c])
315    RealSolution = []
316
317    for k in range(len(Solution)):
318        if abs(Solution[k].imag) < 1e-15:
319            RealSolution.append(Solution[k].real)
320
321    return RealSolution
322
323
324def SolverQuartic(a, b, c, d, e):
325    """
326    Solve the quartic equation a*x^4 + b*x^3 +c*x^2 + d*x + e = 0 ; keep only real solutions
327    """
328    Solution = np.roots([a, b, c, d, e])
329    RealSolution = []
330
331    for k in range(len(Solution)):
332        if abs(Solution[k].imag) < 1e-15:
333            RealSolution.append(Solution[k].real)
334
335    return RealSolution
336
337
338def KeepPositiveSolution(SolutionList):
339    """
340    Keep only positive solution (numbers) in the list
341    """
342    PositiveSolutionList = []
343    epsilon = 1e-12
344    for k in SolutionList:
345        if k > epsilon:
346            PositiveSolutionList.append(k)
347
348    return PositiveSolutionList
349
350
351def KeepNegativeSolution(SolutionList):
352    """
353    Keep only positive solution (numbers) in the list
354    """
355    NegativeSolutionList = []
356    epsilon = -1e-12
357    for k in SolutionList:
358        if k < epsilon:
359            NegativeSolutionList.append(k)
360
361    return NegativeSolutionList
362
363
364# %% Point geometry tools
365def ClosestPoint(A: Point, Points: PointArray):
366    """
367    Given a reference point A and an array of points, return the index of the point closest to A
368    """
369    distances = (Points-A).norm
370    return np.argmin(distances)
371
372def DiameterPointArray(Points: PointArray):
373    """
374    Return the diameter of the smallest circle (for 2D points) 
375    or sphere (3D points) including all the points.
376    """
377    if len(Points) == 0:
378        return None
379    return float(np.ptp(Points, axis=0).max())
380
381def CentrePointList(Points):
382    """
383    Shift all 2D-points in PointList so as to center the point-cloud on the origin [0,0].
384    """
385    return Points - np.mean(Points, axis=0)
386
387# %% Solid object orientation
388
389def RotateSolid(Object, q):
390    """
391    Rotate object around basepoint by quaternion q
392    """
393    Object.q = q*Object.q
394
395def TranslateSolid(Object, T):
396    """
397    Translate object by vector T
398    """
399    Object.r = Object.r + T
400
401def RotateSolidAroundInternalPointByQ(Object, q, P):
402    """
403    Rotate object around P by quaternion q where P is in the object's frame
404    """
405    pass #TODO
406
407def RotateSolidAroundExternalPointByQ(Object, q, P):
408    """Rotate object around P by quaternion q, where P is in the global frame"""
409    pass #TODO
410
411
412# %% Signed distance functions 
413
414def SDF_Rectangle(Point, SizeX, SizeY):
415    """Signed distance function for a rectangle centered at the origin"""
416    d = np.abs(Point[:2]) - np.array([SizeX, SizeY]) / 2
417    return (np.linalg.norm(np.maximum(d, 0)) + np.min(np.max(d, 0)))/2
418
419def SDF_Circle(Point, Radius):
420    """Signed distance function for a circle centered at the origin"""
421    return np.linalg.norm(Point[:2]) - Radius
422
423def Union_SDF(SDF1, SDF2):
424    """Union of two signed distance functions"""
425    return np.minimum(SDF1, SDF2)
426
427def Difference_SDF(SDF1, SDF2):
428    """Difference of two signed distance functions"""
429    return np.maximum(SDF1, -SDF2)
430
431def Intersection_SDF(SDF1, SDF2):
432    """Intersection of two signed distance functions"""
433    return np.maximum(SDF1, SDF2)
434
435
436
437# %% Quaternion calculations
438def QRotationAroundAxis(Axis, Angle):
439    """
440    Return quaternion for rotation by Angle (in rad) around Axis
441    """
442    rot_axis = Normalize(np.array([0.0] + Axis))
443    axis_angle = (Angle * 0.5) * rot_axis
444    qlog = np.quaternion(*axis_angle)
445    q = np.exp(qlog)
446    return q
447
448def QRotationVector2Vector(Vector1, Vector2):
449    """
450    Return a possible quaternion (among many) that would rotate Vector1 into Vector2.
451    Undefined behavior, use with caution. There is no unique quaternion that rotates one vector into another.
452    """
453    Vector1 = Normalize(Vector1)
454    Vector2 = Normalize(Vector2)
455    a = np.cross(Vector1, Vector2)
456    return np.quaternion(1 + np.dot(Vector1, Vector2), *a).normalized()
457
458def QRotationVectorPair2VectorPair(InitialVector1, Vector1, InitialVector2, Vector2):
459    """
460    Return the only quaternion that rotates InitialVector1 to Vector1 and InitialVector2 to Vector2.
461    Please ensure orthogonality two input and two output vectors.
462    """
463    Vector1 = Normalize(Vector1)
464    Vector2 = Normalize(Vector2)
465    Vector3 = Normalize(np.cross(Vector1,Vector2))
466    InitialVector1 = Normalize(InitialVector1)
467    InitialVector2 = Normalize(InitialVector2)
468    InitialVector3 = Normalize(np.cross(InitialVector1,InitialVector2))
469    rot_2Initial = np.zeros((3,3))
470    rot_2Initial[:,0] = InitialVector1
471    rot_2Initial[:,1] = InitialVector2
472    rot_2Initial[:,2] = InitialVector3
473    rot_2Final = np.zeros((3,3))
474    rot_2Final[:,0] = Vector1
475    rot_2Final[:,1] = Vector2
476    rot_2Final[:,2] = Vector3
477    q2Init = quaternion.from_rotation_matrix(rot_2Initial)
478    q2Fin = quaternion.from_rotation_matrix(rot_2Final)
479    return (q2Fin/q2Init).normalized()
480
481
482# %% RayList stuff
483def RotationRayList(RayList, q):
484    """Like RotationPointList but with a list of Ray objects"""
485    return [i.rotate(q) for i in RayList]
486
487def TranslationRayList(RayList, T):
488    """Translate a RayList by vector T"""
489    return [i.translate(T) for i in RayList]