import numpy as np def warnlogic(warnvek): warnvek = np.asarray(warnvek, dtype=np.float64).ravel() halfN = int(np.round(0.5 * len(warnvek))) wind1 = np.where(warnvek > 0)[0] wind2 = np.where(warnvek != 0)[0] wind = np.concatenate([wind1, wind2]) if (len(wind1) > 0 or len(wind2) > 0) else np.array([], dtype=int) if len(wind) == 0: warnall = 0 warnx = 0 warny = 0 elif np.all(wind < halfN): warnall = warnvek[wind[0]] warnx = warnall warny = 0 elif np.all(wind >= halfN): warnall = warnvek[wind[0]] warnx = 0 warny = warnall else: warnall = warnvek[wind[0]] idx_x = np.where(wind < halfN)[0] idx_y = np.where(wind >= halfN)[0] warnx = warnvek[wind[idx_x[0]]] if len(idx_x) > 0 else 0 warny = warnvek[wind[idx_y[0]]] if len(idx_y) > 0 else 0 if len(warnvek) % 2 == 1: warnx = np.array([]) warny = np.array([]) return warnall, warnx, warny def _calcellintersect(a, b, x0i, y0i, m, b0, phi, err_in): phi_tmp = phi % (2 * np.pi) radix = np.sqrt(a**2 * (b**4 - (b * (m * x0i - y0i + b0))**2) + (a**4 * (b * m)**2)) denom = b**2 + (m * a)**2 x1 = (a**2 * m * (y0i - b0) - radix + x0i * b**2) / denom y1 = m * x1 + b0 x2 = (a**2 * m * (y0i - b0) + radix + x0i * b**2) / denom y2 = m * x2 + b0 x = 0.0 y = 0.0 err = err_in if x1 != x2: if ((phi_tmp < np.pi / 2) and (phi_tmp > 0)) or ((phi_tmp > 3 * np.pi / 2) and (phi_tmp < 2 * np.pi)): if x1 > 0: x = x1 y = y1 else: x = x2 y = y2 err = 0 elif (phi_tmp < 3 * np.pi / 2) and (phi_tmp > np.pi / 2): if x1 < 0: x = x1 y = y1 else: x = x2 y = y2 err = 0 if not np.isreal(radix): err = 91 return x, y, err def calcminmaxwth(Layer, DMOuter, MinMaxAng, conf, EPFlag): Layer = np.asarray(Layer, dtype=np.float64).copy() DMOuter = np.asarray(DMOuter, dtype=np.float64).copy() MinMaxAng = np.asarray(MinMaxAng, dtype=np.float64).copy() minwthflag = conf[196] if len(conf) > 196 else 0 nangle = int(2 ** np.floor(np.log2(conf[197] if len(conf) > 197 else 16))) dangle = 360.0 / nangle addangle = np.arange(0, 360, dangle) if EPFlag: mask = (addangle % 45) != 0 addangle = addangle[mask] else: mask = (addangle % 90) != 0 addangle = addangle[mask] addangle = addangle / 180.0 * np.pi InitStepL = 0.1 MINWTHERROR = 92 aa = DMOuter[0, 1] / 2.0 ba = DMOuter[0, 2] / 2.0 ai = aa - (Layer[0, 0] + Layer[0, 1]) / 2.0 bi = ba - (Layer[0, 2] + Layer[0, 3]) / 2.0 x0i = Layer[0, 11] y0i = Layer[0, 12] phi = (addangle - np.pi / 2) % (2 * np.pi) lenphi = len(phi) err = np.ones(lenphi) * 255 if (aa == 0) or (ba == 0) or (ai == 0) or (bi == 0): Layer[1, 5:7] = MINWTHERROR MinMaxAng[1, 0:2] = MINWTHERROR return Layer, MinMaxAng t = np.arctan(aa / ba * np.tan(phi)) mask_phi = (phi > np.pi / 2) & (phi < 3 * np.pi / 2) t[mask_phi] = t[mask_phi] + np.pi ag = np.tan(phi) xa = aa * np.cos(t) ya = ba * np.sin(t) ra = np.sqrt(xa * xa + ya * ya) xi = np.zeros(lenphi) yi = np.zeros(lenphi) ri = np.zeros(lenphi) for k in range(lenphi): xi[k], yi[k], err[k] = _calcellintersect(ai, bi, x0i, y0i, ag[k], 0, phi[k], err[k]) ri[k] = np.sqrt(xi[k]**2 + yi[k]**2) Wt = ra - ri WtMin = np.min(Wt) idxmin = np.argmin(Wt) errmin = err[idxmin] if not np.all(np.isreal(Wt)): errmin = 91 if errmin == 0: xi_min = xi[idxmin] yi_min = yi[idxmin] if yi[idxmin] < y0i: sig = -1 else: sig = 1 run = 1 StepL = InitStepL while (run == 1) and np.isreal(np.sqrt(1 - ((xi[idxmin] - StepL - x0i) / ai)**2)): yWtMin = y0i + bi * sig * np.sqrt(1 - ((xi[idxmin] - StepL - x0i) / ai)**2) WtMin_tmp = np.sqrt((xa[idxmin] - (xi[idxmin] - StepL))**2 + (ya[idxmin] - yWtMin)**2) if WtMin_tmp < WtMin: WtMin = WtMin_tmp xi_min = xi[idxmin] - StepL yi_min = yWtMin StepL = StepL + StepL / 2.0 else: StepL = StepL / 4.0 if StepL < 1e-3: run = 0 if not (WtMin < Wt[idxmin]): run = 1 StepL = InitStepL WtMin = Wt[idxmin] while (run == 1) and np.isreal(np.sqrt(1 - ((xi[idxmin] + StepL - x0i) / ai)**2)): yWtMin = y0i + bi * sig * np.sqrt(1 - ((xi[idxmin] + StepL - x0i) / ai)**2) WtMin_tmp = np.sqrt((xa[idxmin] - (xi[idxmin] + StepL))**2 + (ya[idxmin] - yWtMin)**2) if WtMin_tmp < WtMin: WtMin = WtMin_tmp xi_min = xi[idxmin] + StepL yi_min = yWtMin StepL = StepL + StepL / 2.0 else: StepL = StepL / 4.0 if StepL < 1e-3: run = 0 if WtMin < Layer[0, 5]: MinMaxAng[0, 0] = ((phi[idxmin] + np.pi / 2) * 180.0 / np.pi) % 360 if minwthflag: Layer[0, 5] = WtMin else: MinMaxAng[1, 0] = MINWTHERROR Layer[1, 5] = MINWTHERROR WtMax = np.max(Wt) idxmax = np.argmax(Wt) errmax = err[idxmax] if errmax == 0: if WtMax > Layer[0, 6]: MinMaxAng[0, 1] = ((phi[idxmax] + np.pi / 2) * 180.0 / np.pi) % 360 if minwthflag: Layer[0, 6] = WtMax else: MinMaxAng[1, 1] = MINWTHERROR Layer[1, 6] = MINWTHERROR return Layer, MinMaxAng def updateMinMaxWTH(Layer, DMOuter, DMInner, BisectionWith, StOK, StDefault, StNovalue, reduceavgon, StWait=249): Layer = np.asarray(Layer, dtype=np.float64).copy() DMOuter = np.asarray(DMOuter, dtype=np.float64).copy() DMInner = np.asarray(DMInner, dtype=np.float64).copy() BisectionWith = np.asarray(BisectionWith, dtype=np.float64).copy() def _ok(s): return s in (StOK, StWait) MinMaxAng = StDefault * np.ones((3, 2), dtype=np.float64) warnvek = np.array([DMOuter[2, 0], DMInner[2, 0], Layer[2, 4]]) warnall, _, _ = warnlogic(warnvek) MinMaxAng[2, :] = warnall if (_ok(DMOuter[1, 1]) and _ok(DMOuter[1, 2]) and _ok(DMInner[1, 1]) and _ok(DMInner[1, 2]) and _ok(Layer[1, 11]) and _ok(Layer[1, 12])): if (_ok(Layer[1, 0]) and _ok(Layer[1, 1]) and _ok(Layer[1, 2]) and _ok(Layer[1, 3])): if (_ok(BisectionWith[1, 0]) and _ok(BisectionWith[1, 1]) and _ok(BisectionWith[1, 2]) and _ok(BisectionWith[1, 3])): vals = np.array([Layer[0, 2], BisectionWith[0, 0], Layer[0, 1], BisectionWith[0, 1], Layer[0, 3], BisectionWith[0, 2], Layer[0, 0], BisectionWith[0, 3]]) WtMin = np.min(vals) IndMin = np.argmin(vals) MinMaxAng[0, 0] = 45 * IndMin MinMaxAng[1, 0] = StOK WtMax = np.max(vals) IndMax = np.argmax(vals) MinMaxAng[0, 1] = 45 * IndMax MinMaxAng[1, 1] = StOK else: vals = np.array([Layer[0, 2], Layer[0, 1], Layer[0, 3], Layer[0, 0]]) WtMin = np.min(vals) IndMin = np.argmin(vals) MinMaxAng[0, 0] = 90 * IndMin MinMaxAng[1, 0] = StOK WtMax = np.max(vals) IndMax = np.argmax(vals) MinMaxAng[0, 1] = 90 * IndMax MinMaxAng[1, 1] = StOK Layer[0, 5] = WtMin Layer[1, 5] = StOK Layer[0, 6] = WtMax Layer[1, 6] = StOK else: Layer[0, 5] = 0 Layer[1, 5] = StNovalue Layer[0, 6] = 0 Layer[1, 6] = StNovalue MinMaxAng[0, 0] = 0 MinMaxAng[1, 0] = StNovalue MinMaxAng[0, 1] = 0 MinMaxAng[1, 1] = StNovalue else: if not _ok(DMOuter[1, 1]): TempSt = DMOuter[1, 1] elif not _ok(DMOuter[1, 2]): TempSt = DMOuter[1, 2] elif not _ok(DMInner[1, 1]): TempSt = DMInner[1, 1] elif not _ok(DMInner[1, 2]): TempSt = DMInner[1, 2] elif not _ok(Layer[1, 11]): TempSt = Layer[1, 11] elif not _ok(Layer[1, 12]): TempSt = Layer[1, 12] else: TempSt = StDefault BisectionWith[0, 0] = 0 BisectionWith[1, 0] = TempSt BisectionWith[0, 1] = 0 BisectionWith[1, 1] = TempSt BisectionWith[0, 2] = 0 BisectionWith[1, 2] = TempSt BisectionWith[0, 3] = 0 BisectionWith[1, 3] = TempSt if not reduceavgon: Layer[0, 5] = 0 Layer[1, 5] = TempSt Layer[0, 6] = 0 Layer[1, 6] = TempSt MinMaxAng[0, 0] = 0 MinMaxAng[1, 0] = TempSt MinMaxAng[0, 1] = 0 MinMaxAng[1, 1] = TempSt return Layer, BisectionWith, MinMaxAng def updateMinMaxWTHall(Layer, Wt, MinMaxAng, conf, phi, Err): minwthflag = conf[196] if len(conf) > 196 else 0 MINWTHERROR = 92 Wt = np.asarray(Wt, dtype=np.float64).copy() Err = np.asarray(Err, dtype=np.float64) if Err.shape == Wt.shape: Wt[Err != 0] = np.nan MinMaxAng = np.asarray(MinMaxAng, dtype=np.float64).copy() Layer = np.asarray(Layer, dtype=np.float64).copy() if Wt.ndim == 1: Wt = Wt.reshape(-1, 1) n_phi, n_col = Wt.shape WtMin1 = np.full(n_col, np.nan) idxmin1 = np.zeros(n_col, dtype=int) for j in range(n_col): col = Wt[:, j] valid = ~np.isnan(col) if np.any(valid): idx = np.argmin(col[valid]) orig_idx = np.where(valid)[0][idx] WtMin1[j] = col[orig_idx] idxmin1[j] = orig_idx valid_min = ~np.isnan(WtMin1) if np.any(valid_min): idxmin = np.argmin(WtMin1[valid_min]) col_idx = np.where(valid_min)[0][idxmin] WtMin = WtMin1[col_idx] phi_idx = idxmin1[col_idx] if WtMin < Layer[0, 5]: angle_rad = phi[phi_idx] + col_idx * np.pi / 2 MinMaxAng[0, 0] = np.mod(angle_rad * 180 / np.pi, 360) if minwthflag: Layer[0, 5] = WtMin WtMax1 = np.full(n_col, np.nan) idxmax1 = np.zeros(n_col, dtype=int) for j in range(n_col): col = Wt[:, j] valid = ~np.isnan(col) if np.any(valid): idx = np.argmax(col[valid]) orig_idx = np.where(valid)[0][idx] WtMax1[j] = col[orig_idx] idxmax1[j] = orig_idx valid_max = ~np.isnan(WtMax1) if np.any(valid_max): idxmax = np.argmax(WtMax1[valid_max]) col_idx = np.where(valid_max)[0][idxmax] WtMax = WtMax1[col_idx] phi_idx = idxmax1[col_idx] if WtMax > Layer[0, 6]: angle_rad = phi[phi_idx] + col_idx * np.pi / 2 MinMaxAng[0, 1] = np.mod(angle_rad * 180 / np.pi, 360) if minwthflag: Layer[0, 6] = WtMax return Layer, MinMaxAng