# packages of various functions. from copy import copy from math import * def distance(p, q): x = p[0] - q[0] y = p[1] - q[1] z = p[2] - q[2] return sqrt(x ** 2 + y ** 2 + z ** 2) class Spherical: @staticmethod def to(p, center): rho = distance(p, center) p = copy(p); p[0] -= center[0]; p[1] -= center[1]; p[2] -= center[2] phi = atan2(p[1], p[0]) try: theta = acos(p[2] / rho) except ZeroDivisionError: theta = acos(p[2] / 1e-8) return rho, phi, theta @staticmethod def xyz(rho, phi, theta, center): x = rho * cos(phi) * sin(theta) + center[0] y = rho * sin(phi) * sin(theta) + center[1] z = rho * cos(theta) + center[2] return [ x, y, z ] def centroid(pts): x = 0. y = 0. z = 0. for p in pts: x += p[0] y += p[1] z += p[2] x /= len(pts) y /= len(pts) z /= len(pts) return [ x, y, z ] # for elliptical coords class Ellipse: def __init__(self, pos, axis): self.__pos = copy(pos) halfAxis = copy(axis) for i in range(3): halfAxis[i] /= 2. self.__inverse = halfAxis[0] < halfAxis[1] self.__halfAxis = halfAxis # eccentricity a = 0; b = 1; if halfAxis[a] < halfAxis[b]: b = 0; a = 1 self.__eccen = sqrt(halfAxis[a] ** 2 - halfAxis[b] ** 2) / halfAxis[a] def R(self, phi): return self.__halfAxis[0] / sqrt(1 - (self.__eccen * sin(phi)) ** 2) # from elliptical to cartesian def toXYZ(self, h, lamb, phi): N = self.R(phi) XYProj = (N + h) * cos(phi) p = [ XYProj * cos(lamb), XYProj * sin(lamb), ((1 - self.__eccen ** 2) * N + h) * sin(phi) ] if self.__inverse: aux = p[0]; p[0] = p[1]; p[1] = aux for i in range(3): p[i] += self.__pos[i] return p # from cartesian to elliptical def toElliptical(self, pt): x = pt[0] - self.__pos[0] y = pt[1] - self.__pos[1] z = pt[2] - self.__pos[2] if self.__inverse: aux = y; y = x; x = aux lamb = atan2(y, x) p = sqrt(x ** 2 + y ** 2) try: phi = atan(z / ((1 - self.__eccen ** 2) * p)) except ZeroDivisionError: phi = atan(z / 1e-8) MAXIT = int(1e+4) for i in range(MAXIT): phi1 = phi N = self.R(phi) h = p / cos(phi) - N try: phi = atan(z / ((1 - self.__eccen ** 2 * N / (N + h)) * p)) except ZeroDivisionError: phi = atan(z / 1e-8) if abs(phi - phi1) < 1e-8: break return h, lamb, phi def getZ(self, pt): x = pt[0]; y = pt[1] try: return self.__pos[2] + self.__halfAxis[2] * sqrt(1 - ((x - self.__pos[0]) / self.__halfAxis[0]) ** 2 - ((y - self.__pos[1]) / self.__halfAxis[1]) ** 2) except ValueError: return None def normalRadius(self, pt): r = 0. for i in range(3): r += ((pt[i] - self.__pos[i]) / self.__halfAxis[i]) ** 2 return r # return versor between two points def versor(p, q): d = distance(p, q) v = [ 0 ] * 3 for i in range(3): v[i] = (p[i] - q[i]) / d return v def ellipseLineIntersec(u, p, center, axis): p = copy(p) A = 0. B = 0. C = -1 v = [] for i in range(3): _ax = axis[i] / 2 A += u[i] ** 2 / _ax ** 2 B += 2 * u[i] * (p[i] - center[i]) / _ax ** 2 C += (p[i] - center[i]) ** 2 / _ax ** 2 delta = B ** 2 - 4 * A * C t0 = (-B + sqrt(delta)) / (2 * A) t1 = (-B - sqrt(delta)) / (2 * A) t = t1 if abs(t0) < abs(t1): t = t0 for i in range(3): p[i] += t * u[i] return p def ellipseIntersec(p, center, axis): e = Ellipse(center, axis) h, lamb, phi = e.toElliptical(p) u = [ sin(lamb) * cos(phi), cos(lamb) * cos(phi), sin(phi) ] p = ellipseLineIntersec(u, p, center, axis) return p, lamb, phi