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
153# %% More traditional vector operations that don't belong in the classes
154def Normalize(vector):
155    """
156    Normalize Vector.
157    Obsolete, use use the mgeo.Vector class instead as it has a `normalize` method.
158    """
159    return vector / np.linalg.norm(vector)
160
161def VectorPerpendicular(vector):
162    """
163    Find a perpendicular 3D vector in some arbitrary direction
164    Undefined behavior, use with caution. There is no unique perpendicular vector to a 3D vector.
165    """
166    logger.warning("VectorPerpendicular is undefined behavior. There is no unique perpendicular vector to a 3D vector.")
167    if abs(vector[0]) < 1e-15:
168        return Vector([1, 0, 0])
169    if abs(vector[1]) < 1e-15:
170        return Vector([0, 1, 0])
171    if abs(vector[2]) < 1e-15:
172        return Vector([0, 0, 1])
173
174    # set arbitrarily a = b =1
175    return Vector([1, 1, -1.0 * (vector[0] + vector[1]) / vector[2]]).normalized()
176
177def AngleBetweenTwoVectors(U, V):
178    """
179    Return the angle in radians between the vectors U and V ; formula from W.Kahan
180    Value in radians between 0 and pi.
181    """
182    u = np.linalg.norm(U)
183    v = np.linalg.norm(V)
184    return 2 * np.arctan2(np.linalg.norm(U * v - V * u), np.linalg.norm(U * v + V * u))
185
186def SymmetricalVector(V, SymmetryAxis):
187    """
188    Return the symmetrical vector to V
189    """
190    q = QRotationAroundAxis(SymmetryAxis, np.pi)
191    return V.rotate(q)
192
193def normal_add(N1, N2):
194    """
195    Simple function that takes in two normal vectors of a deformation and calculates
196    the total normal vector if the two deformations were individually applied.
197    Be very careful, this *only* works when the surface is z = f(x,y) and when 
198    the deformation is small.
199    Might be made obsolete when shifting to a modernised deformation system.
200    """
201    normal1 = N1.normalized()
202    normal2 = N2.normalized()
203    grad1 = -normal1[:2] / normal1[2]
204    grad2 = -normal2[:2] / normal2[2]
205    grad = grad1 + grad2
206    total_normal = np.append(-grad, 1)
207    return Vector(total_normal).normalized()
208
209# %% Intersection finding
210def IntersectionLinePlane(A, u, P, n):
211    """
212    Return the intersection point between a line and a plane.
213    A is a point of the line, u a vector of the line ; P is a point of the plane, n a normal vector
214    Line's equation : OM = u*t + OA , t a real
215    Plane's equation : n.OP - n.OM = 0
216    """
217    t = np.dot(n, -A + P) / np.dot(u, n)
218    I = u * t + A
219    return I
220
221def IntersectionRayListZPlane(RayList, Z=np.array([0])):
222    """
223    Return the intersection of a list of rays with a different planes with equiations z = Z[i]
224    Basically, by default it returns the intersection of the rays with the Z=0 plane but you can 
225    give it a few values of Z and it should be faster than calling it multiple times.
226    This should let us quickly find the optimal position of the detector as well as trace the caustics.
227    If a ray does not intersect the plane... it should replace that point with a NaN. 
228    """
229    Positions = np.vstack([i.point for i in RayList])
230    Vectors = np.vstack([i.vector for i in RayList])
231    non_zero = Vectors[:,2] != 0
232    Positions = Positions[non_zero]
233    Vectors = Vectors[non_zero]
234    Z = Z[:, np.newaxis]
235    A = Positions[:,2]-Z
236    B = -Vectors[:,2]
237    #times = (Positions[:,2]-Z)/Vectors[:,2]
238    #return A,B
239    with np.errstate(divide='ignore', invalid='ignore'):
240        times = np.divide(A, B, where=(B != 0), out=np.full_like(A, np.nan))
241        #times[times < 0] = np.nan  # Set negative results to NaN
242    #return times
243    #positive_times = times >= 0
244    intersect_positions = Positions[:, :2] + times[:, :, np.newaxis] * Vectors[:, :2]
245    result = []
246    for i in range(Z.shape[0]):
247        # For each plane, we find the intersection points
248        #valid_intersections = intersect_positions[i][positive_times[i]]
249        valid_intersections = intersect_positions[i]
250        result.append(PointArray(valid_intersections))
251    return result
252
253
254# %% Geometrical utilities for plotting
255def SpiralVogel(NbPoint, Radius):
256    """
257    Return a NbPoint x 2 matrix of 2D points representative of Vogel's spiral with radius Radius
258    Careful, contrary to most of the code, this is *not* in the 
259    ARTcore.Vector or ARTcore.Point format. It is a simple numpy array. 
260    The reason is that this is a utility function that can be used both to define directions
261    and to generate grids of points.
262    """
263    GoldenAngle = np.pi * (3 - np.sqrt(5))
264    r = np.sqrt(np.arange(NbPoint) / NbPoint) * Radius
265
266    theta = GoldenAngle * np.arange(NbPoint)
267
268    Matrix = np.zeros((NbPoint, 2))
269    Matrix[:, 0] = np.cos(theta)
270    Matrix[:, 1] = np.sin(theta)
271    Matrix = Matrix * r.reshape((NbPoint, 1))
272
273    return Matrix
274
275def find_hull(points):
276    """
277    Find the convex hull of a set of points using a greedy algorithm.
278    This is used to create a polygon that encloses the points.
279    """
280    # start from leftmost point
281    current_point = min(range(len(points)), key=lambda i: points[i][0])
282    # initialize hull with current point
283    hull = [current_point]
284    # initialize list of linked points
285    linked = []
286    # continue until all points have been linked
287    while len(linked) < len(points) - 1:
288        # initialize minimum distance and closest point
289        min_distance = math.inf
290        closest_point = None
291        # find closest unlinked point to current point
292        for i, point in enumerate(points):
293            if i not in linked:
294                distance = math.dist(points[current_point], point)
295                if distance < min_distance:
296                    min_distance = distance
297                    closest_point = i
298        # add closest point to hull and linked list
299        hull.append(closest_point)
300        linked.append(closest_point)
301        # update current point
302        current_point = closest_point
303    # add link between last point and first point
304    hull.append(hull[0])
305    # convert hull to a list of pairs of indices
306    indices = [[hull[i], hull[i + 1]] for i in range(len(hull) - 1)]
307    return indices
308
309
310# %% Solvers and utilities for solving equations
311def SolverQuadratic(a, b, c):
312    """
313    Solve the quadratic equation a*x^2 + b*x +c = 0 ; keep only real solutions
314    """
315    Solution = np.roots([a, b, c])
316    RealSolution = []
317
318    for k in range(len(Solution)):
319        if abs(Solution[k].imag) < 1e-15:
320            RealSolution.append(Solution[k].real)
321
322    return RealSolution
323
324
325def SolverQuartic(a, b, c, d, e):
326    """
327    Solve the quartic equation a*x^4 + b*x^3 +c*x^2 + d*x + e = 0 ; keep only real solutions
328    """
329    Solution = np.roots([a, b, c, d, e])
330    RealSolution = []
331
332    for k in range(len(Solution)):
333        if abs(Solution[k].imag) < 1e-15:
334            RealSolution.append(Solution[k].real)
335
336    return RealSolution
337
338
339def KeepPositiveSolution(SolutionList):
340    """
341    Keep only positive solution (numbers) in the list
342    """
343    PositiveSolutionList = []
344    epsilon = 1e-12
345    for k in SolutionList:
346        if k > epsilon:
347            PositiveSolutionList.append(k)
348
349    return PositiveSolutionList
350
351
352def KeepNegativeSolution(SolutionList):
353    """
354    Keep only positive solution (numbers) in the list
355    """
356    NegativeSolutionList = []
357    epsilon = -1e-12
358    for k in SolutionList:
359        if k < epsilon:
360            NegativeSolutionList.append(k)
361
362    return NegativeSolutionList
363
364
365# %% Point geometry tools
366def ClosestPoint(A: Point, Points: PointArray):
367    """
368    Given a reference point A and an array of points, return the index of the point closest to A
369    """
370    distances = (Points-A).norm
371    return np.argmin(distances)
372
373def DiameterPointArray(Points: PointArray):
374    """
375    Return the diameter of the smallest circle (for 2D points) 
376    or sphere (3D points) including all the points.
377    """
378    if len(Points) == 0:
379        return None
380    return float(np.ptp(Points, axis=0).max())
381
382def CentrePointList(Points):
383    """
384    Shift all 2D-points in PointList so as to center the point-cloud on the origin [0,0].
385    """
386    return Points - np.mean(Points, axis=0)
387
388# %% Solid object orientation
389
390def RotateSolid(Object, q):
391    """
392    Rotate object around basepoint by quaternion q
393    """
394    Object.q = q*Object.q
395
396def TranslateSolid(Object, T):
397    """
398    Translate object by vector T
399    """
400    Object.r = Object.r + T
401
402def RotateSolidAroundInternalPointByQ(Object, q, P):
403    """
404    Rotate object around P by quaternion q where P is in the object's frame
405    """
406    pass #TODO
407
408def RotateSolidAroundExternalPointByQ(Object, q, P):
409    """Rotate object around P by quaternion q, where P is in the global frame"""
410    pass #TODO
411
412
413# %% Signed distance functions 
414
415def SDF_Rectangle(Point, SizeX, SizeY):
416    """Signed distance function for a rectangle centered at the origin"""
417    d = np.abs(Point[:2]) - np.array([SizeX, SizeY]) / 2
418    return (np.linalg.norm(np.maximum(d, 0)) + np.min(np.max(d, 0)))/2
419
420def SDF_Circle(Point, Radius):
421    """Signed distance function for a circle centered at the origin"""
422    return np.linalg.norm(Point[:2]) - Radius
423
424def Union_SDF(SDF1, SDF2):
425    """Union of two signed distance functions"""
426    return np.minimum(SDF1, SDF2)
427
428def Difference_SDF(SDF1, SDF2):
429    """Difference of two signed distance functions"""
430    return np.maximum(SDF1, -SDF2)
431
432def Intersection_SDF(SDF1, SDF2):
433    """Intersection of two signed distance functions"""
434    return np.maximum(SDF1, SDF2)
435
436
437
438# %% Quaternion calculations
439def QRotationAroundAxis(Axis, Angle):
440    """
441    Return quaternion for rotation by Angle (in rad) around Axis
442    """
443    rot_axis = Normalize(np.array([0.0] + Axis))
444    axis_angle = (Angle * 0.5) * rot_axis
445    qlog = np.quaternion(*axis_angle)
446    q = np.exp(qlog)
447    return q
448
449def QRotationVector2Vector(Vector1, Vector2):
450    """
451    Return a possible quaternion (among many) that would rotate Vector1 into Vector2.
452    Undefined behavior, use with caution. There is no unique quaternion that rotates one vector into another.
453    """
454    Vector1 = Normalize(Vector1)
455    Vector2 = Normalize(Vector2)
456    a = np.cross(Vector1, Vector2)
457    return np.quaternion(1 + np.dot(Vector1, Vector2), *a).normalized()
458
459def QRotationVectorPair2VectorPair(InitialVector1, Vector1, InitialVector2, Vector2):
460    """
461    Return the only quaternion that rotates InitialVector1 to Vector1 and InitialVector2 to Vector2.
462    Please ensure orthogonality two input and two output vectors.
463    """
464    Vector1 = Normalize(Vector1)
465    Vector2 = Normalize(Vector2)
466    Vector3 = Normalize(np.cross(Vector1,Vector2))
467    InitialVector1 = Normalize(InitialVector1)
468    InitialVector2 = Normalize(InitialVector2)
469    InitialVector3 = Normalize(np.cross(InitialVector1,InitialVector2))
470    rot_2Initial = np.zeros((3,3))
471    rot_2Initial[:,0] = InitialVector1
472    rot_2Initial[:,1] = InitialVector2
473    rot_2Initial[:,2] = InitialVector3
474    rot_2Final = np.zeros((3,3))
475    rot_2Final[:,0] = Vector1
476    rot_2Final[:,1] = Vector2
477    rot_2Final[:,2] = Vector3
478    q2Init = quaternion.from_rotation_matrix(rot_2Initial)
479    q2Fin = quaternion.from_rotation_matrix(rot_2Final)
480    return (q2Fin/q2Init).normalized()
481
482
483# %% RayList stuff
484def RotationRayList(RayList, q):
485    """Like RotationPointList but with a list of Ray objects"""
486    return [i.rotate(q) for i in RayList]
487
488def TranslationRayList(RayList, T):
489    """Translate a RayList by vector T"""
490    return [i.translate(T) for i in RayList]