def test_gauss_quadrature_dynamic(verbose = False):
n = 5
A = mp.randmatrix(2 * n, 1)
def F(x):
r = 0
for i in xrange(len(A) - 1, -1, -1):
r = r * x + A[i]
return r
def run(qtype, FW, R, alpha = 0, beta = 0):
X, W = mp.gauss_quadrature(n, qtype, alpha = alpha, beta = beta)
a = 0
for i in xrange(len(X)):
a += W[i] * F(X[i])
b = mp.quad(lambda x: FW(x) * F(x), R)
c = mp.fabs(a - b)
if verbose:
print(qtype, c, a, b)
assert c < 1e-5
run("legendre", lambda x: 1, [-1, 1])
run("legendre01", lambda x: 1, [0, 1])
run("hermite", lambda x: mp.exp(-x*x), [-mp.inf, mp.inf])
run("laguerre", lambda x: mp.exp(-x), [0, mp.inf])
run("glaguerre", lambda x: mp.sqrt(x)*mp.exp(-x), [0, mp.inf], alpha = 1 / mp.mpf(2))
run("chebyshev1", lambda x: 1/mp.sqrt(1-x*x), [-1, 1])
run("chebyshev2", lambda x: mp.sqrt(1-x*x), [-1, 1])
run("jacobi", lambda x: (1-x)**(1/mp.mpf(3)) * (1+x)**(1/mp.mpf(5)), [-1, 1], alpha = 1 / mp.mpf(3), beta = 1 / mp.mpf(5) )
def two_largest(num_list):
largest_num = mp.mpf(-1e100)
second_largest_num = mp.mpf(-1e99)
for num in num_list:
if num > largest_num:
second_largest_num = largest_num
largest_num = num
if largest_num < num < second_largest_num:
second_largest_num = num
return (largest_num, second_largest_num)
def __init__(self, npts):
# Legendre poly
lp = lambda x: mp.legendre(npts - 1, x)
# Coefficients of lp
cf = mp.taylor(lp, 0, npts - 1)
# Coefficients of dlp/dx
dcf = [i*c for i, c in enumerate(cf[1:], start=1)]
self.points = [mp.mpf(-1)] + mp.polyroots(dcf[::-1]) + [mp.mpf(1)]
self.weights = [2/(npts*(npts - 1)*lp(p)**2) for p in self.points]
def _Interpolate1DNoVelocityLimit(x0, x1, v0, v1, am):
# Check types
if type(x0) is not mp.mpf:
x0 = mp.mpf("{:.15e}".format(x0))
if type(x1) is not mp.mpf:
x1 = mp.mpf("{:.15e}".format(x1))
if type(v0) is not mp.mpf:
v0 = mp.mpf("{:.15e}".format(v0))
if type(v1) is not mp.mpf:
v1 = mp.mpf("{:.15e}".format(v1))
if type(am) is not mp.mpf:
am = mp.mpf("{:.15e}".format(am))
# Check inputs
assert(am > zero)
# Check for an appropriate acceleration direction of the first ramp
d = Sub(x1, x0)
dv = Sub(v1, v0)
difVSqr = Sub(v1**2, v0**2)
if Abs(dv) < epsilon:
if Abs(d) < epsilon:
# Stationary ramp
ramp0 = Ramp(zero, zero, zero, x0)
return ParabolicCurve([ramp0])
else:
dStraight = zero
else:
dStraight = mp.fdiv(difVSqr, Prod([2, mp.sign(dv), am]))
if IsEqual(d, dStraight):
# With the given distance, v0 and v1 can be directly connected using max/min
# acceleration. Here the resulting profile has only one ramp.
a0 = mp.sign(dv) * am
ramp0 = Ramp(v0, a0, mp.fdiv(dv, a0), x0)
return ParabolicCurve([ramp0])
sumVSqr = Add(v0**2, v1**2)
sigma = mp.sign(Sub(d, dStraight))
a0 = sigma * am # acceleration of the first ramp
vp = sigma * mp.sqrt(Add(Mul(pointfive, sumVSqr), Mul(a0, d)))
t0 = mp.fdiv(Sub(vp, v0), a0)
t1 = mp.fdiv(Sub(vp, v1), a0)
ramp0 = Ramp(v0, a0, t0, x0)
assert(IsEqual(ramp0.v1, vp)) # check soundness
ramp1 = Ramp(vp, Neg(a0), t1)
curve = ParabolicCurve([ramp0, ramp1])
assert(IsEqual(curve.d, d)) # check soundness
return curve
def smallest_largest_elements(_matrix):
l = len(_matrix) - 1
smallest_element = mp.mpf(1e100) # initialize to very large number
largest_element = mp.mpf(0)
for i in range(l):
for j in range(l):
num = _matrix[i, j]
if num != 0:
abs_num = fabs(num)
if abs_num < smallest_element:
smallest_element = abs_num
if abs_num > largest_element:
largest_element = abs_num
return largest_element, smallest_element
def ortho_basis_at_mp(self, p, q, r):
a = 2 * p / (1 - r) if r != 1 else 0
b = 2 * q / (1 - r) if r != 1 else 0
c = r
sk = [
mp.mpf(2)**(-k - 0.25) * mp.sqrt(k + 0.5)
for k in range(self.order)
]
pa = [s * jp for s, jp in zip(sk, jacobi(self.order - 1, 0, 0, a))]
pb = [s * jp for s, jp in zip(sk, jacobi(self.order - 1, 0, 0, b))]
ob = []
for i, pi in enumerate(pa):
for j, pj in enumerate(pb):
cij = (1 - c)**(i + j)
pij = pi * pj
pc = jacobi(self.order - max(i, j) - 1, 2 * (i + j + 1), 0, c)
for k, pk in enumerate(pc):
ck = mp.sqrt(2 * (k + j + i) + 3)
ob.append(cij * ck * pij * pk)
return ob
def _evaluate_Tn_mpmath_py(x, Tn):
r"""mpmath implementation of evaluate_Tn()."""
num = len(Tn)
one = mp.one
two = mp.mpf(2)
if x == 1:
return [one] * num
if x == -1:
return [(-one)**k for k in range(num)]
x = mp.mpf(x)
Tn[0] = one
if num > 1:
Tn[1] = x
for n in range(2, num):
Tn[n] = (two * x) * Tn[n - 1] - Tn[n - 2]
return Tn
def _scheme_from_rc_mpmath(alpha, beta):
# Create vector cut of the first value of beta
n = len(alpha)
b = mp.zeros(n, 1)
for i in range(n - 1):
b[i] = mp.sqrt(beta[i + 1])
z = mp.zeros(1, n)
z[0, 0] = 1
d = mp.matrix(alpha)
tridiag_eigen(mp, d, b, z)
# nx1 matrix -> list of mpf
x = numpy.array([mp.mpf(sympy.N(xx, mp.dps)) for xx in d])
w = numpy.array([mp.mpf(sympy.N(beta[0], mp.dps)) * mp.power(ww, 2) for ww in z])
return x, w
def Trim(self, deltaT):
"""
Trim trims the curve such that it has the duration of self.duration - deltaT.
Trim also takes care of where to trim out the deltaT. However, normally we should not have any problem
since deltaT is expected to be very small. This function is aimed to be used when combining Curves to
get ParabolicCurvesND.
Return True if the operation is successful, False otherwise.
"""
if type(deltaT) is not mp.mpf:
dt = mp.mpf("{:.15e}".format(deltaT))
else:
dt = deltaT
if dt > self.duration:
return False # cannot trim
if Abs(dt) < epsilon:
return True # no trimming needed
if dt < self.ramps[-1].duration:
# trim the last ramp
newDur = Sub(self.ramps[-1].duration, dt)
self.ramps[-1].UpdateDuration(newDur)
self.v1 = self.ramps[-1].v1
return True
else:
# have not decided what to do here yet. This is not likely to happen, though, since
# deltaT is expected to be very small.
return False
def DynamicPathStringToParabolicCurvesND(dynamicpathstring):
dynamicpathstring = dynamicpathstring.strip()
data = dynamicpathstring.split("\n")
ndof = int(data[0])
nlines = ndof + 2 # the number of lines containing the data for 1 ParabolicRampND
curves = [ParabolicCurve() for _ in xrange(ndof)]
nParabolicRampND = len(data)/(nlines)
for iramp in xrange(nParabolicRampND):
curoffset = iramp*nlines
for idof in xrange(ndof):
ramp1ddata = data[curoffset + 2 + idof]
x0, v0, x1, v1, a1, v, a2, tswitch1, tswitch2, ttotal = [mp.mpf(x) for x in ramp1ddata.split(" ")]
ramps = []
ramp0 = Ramp(v0, a1, tswitch1, x0)
if ramp0.duration > epsilon:
ramps.append(ramp0)
ramp1 = Ramp(v, 0, tswitch2 - tswitch1, ramp0.x1)
if ramp1.duration > epsilon:
ramps.append(ramp1)
ramp2 = Ramp(v, a2, ttotal - tswitch2, ramp1.x1)
if ramp2.duration > epsilon:
ramps.append(ramp2)
assert(len(ramps) > 0)
curve = ParabolicCurve(ramps)
curves[idof].Append(curve)
return ParabolicCurvesND(curves)
def jac_ortho_basis_at_mp(self, p, q, r):
a = 2 * p / (1 - r) if r != 1 else 0
b = 2 * q / (1 - r) if r != 1 else 0
c = r
sk = [mp.mpf(2) ** (-k - 0.25) * mp.sqrt(k + 0.5) for k in range(self.order)]
fc = [s * jp for s, jp in zip(sk, jacobi(self.order - 1, 0, 0, a))]
gc = [s * jp for s, jp in zip(sk, jacobi(self.order - 1, 0, 0, b))]
dfc = [s * jp for s, jp in zip(sk, jacobi_diff(self.order - 1, 0, 0, a))]
dgc = [s * jp for s, jp in zip(sk, jacobi_diff(self.order - 1, 0, 0, b))]
ob = []
for i, (fi, dfi) in enumerate(zip(fc, dfc)):
for j, (gj, dgj) in enumerate(zip(gc, dgc)):
h = jacobi(self.order - max(i, j) - 1, 2 * (i + j + 1), 0, c)
dh = jacobi_diff(self.order - max(i, j) - 1, 2 * (i + j + 1), 0, c)
for k, (hk, dhk) in enumerate(zip(h, dh)):
ck = mp.sqrt(2 * (k + j + i) + 3)
tmp = (1 - c) ** (i + j - 1) if i + j > 0 else 1
pijk = 2 * tmp * dfi * gj * hk
qijk = 2 * tmp * fi * dgj * hk
rijk = (
tmp * (a * dfi * gj + b * fi * dgj - (i + j) * fi * gj) * hk
+ (1 - c) ** (i + j) * fi * gj * dhk
)
ob.append([ck * pijk, ck * qijk, ck * rijk])
return ob
def __init__(self, rule):
pts = []
wts = []
rule = re.sub(r'(?<=\))\s*,?\s*(?!$)', r'\n', rule)
rule = re.sub(r'\(|\)|,', '', rule).strip()
rule = rule[1:-1] if rule.startswith('[') else rule
for l in rule.splitlines():
if not l:
continue
# Parse the line
args = [mp.mpf(f) for f in l.split()]
if len(args) == self.ndim:
pts.append(args)
elif len(args) == self.ndim + 1:
pts.append(args[:-1])
wts.append(args[-1])
else:
raise ValueError('Invalid points in quadrature rule')
if len(wts) and len(wts) != len(pts):
raise ValueError('Invalid number of weights')
# Flatten 1D rules
if self.ndim == 1:
pts = [p[0] for p in pts]
# Cast
self.pts = np.array(pts, dtype=np.float)
self.wts = np.array(wts, dtype=np.float)
def EvalVel(self, t):
if type(t) is not mp.mpf:
t = mp.mpf("{:.15e}".format(t))
assert(t >= -epsilon)
assert(t <= self.duration + epsilon)
return Add(self.v0, Mul(self.a, t))
def ortho_basis_at_mp(self, p, q, r):
r = r if r != 1 else r + mp.eps
a = 2*p/(1 - r)
b = 2*q/(1 - r)
c = r
sk = [mp.mpf(2)**(-k - 0.25)*mp.sqrt(k + 0.5)
for k in xrange(self.order)]
pa = [s*jp for s, jp in zip(sk, jacobi(self.order - 1, 0, 0, a))]
pb = [s*jp for s, jp in zip(sk, jacobi(self.order - 1, 0, 0, b))]
ob = []
for i, pi in enumerate(pa):
for j, pj in enumerate(pb):
cij = (1 - c)**(i + j)
pij = pi*pj
pc = jacobi(self.order - max(i, j) - 1, 2*(i + j + 1), 0, c)
for k, pk in enumerate(pc):
ck = mp.sqrt(2*(k + j + i) + 3)
ob.append(cij*ck*pij*pk)
return ob
def EvalAcc(self, t):
if type(t) is not mp.mpf:
t = mp.mpf("{:.15e}".format(t))
assert(t >= -epsilon)
assert(t <= self.duration + epsilon)
return self.a
def _ode1(self,
num,
atol,
use_mp=False,
dps=None,
mat_solver='scipy.solve',
eq_generator=False):
# Solve the ODE
# f'' - a b sin(b x) f' = a b^2 cos(b x) f
# with an exact solution
# f(x) = exp(-a cos(b x)).
# The domain is chosen to avoid symmetries and to test non-trivial
# boundary conditions. This test is designed to be able to perform
# fast tests with floating point precision or slow tests with mpmath
# arbitrary precision arithmetics. All constants are coded such that
# arbitrary precision tests can succeed with an accuracy of up to (for
# example):
# atol = 2e-39 with num=160, dps=50
fl = mp.mpf if use_mp else float
ctx = mp if use_mp else np
sin, cos, exp = ctx.sin, ctx.cos, ctx.exp
with mp.workdps(dps or mp.dps):
a, b = map(fl, (2, 6))
domain = lmap(fl, (0.5, 2))
exact = lambda x: exp(-a * cos(b * x))
v1 = fl(
mp.mpf('7.2426342955875784959555289115078253477587230131548'))
v2 = fl(
mp.mpf('0.18494294304163188136560483509304192452611801175781'))
if eq_generator:
eq = lambda pts: ((-a * b**2 * np.cos(b * pts), -a * b * np.
sin(b * pts), 1), 0)
else:
eq = ((lambda x: -a * b**2 * cos(b * x),
lambda x: -a * b * sin(b * x), 1), 0)
sol = ndsolve(eq=eq,
basis=ChebyBasis(domain=domain, num=num),
boundary_conditions=(
DirichletCondition(x=domain[0], value=v1),
DirichletCondition(x=domain[1], value=v2),
),
use_mp=use_mp,
mat_solver=mat_solver)
f = sol.evaluator(use_mp)
pts = np.linspace(float(sol.domain[0]), float(sol.domain[1]), 50)
max_err = max(map(lambda x: abs(exact(x) - f(x)), pts))
self.assertLessEqual(max_err, atol)
def EvalAcc(self, t):
if type(t) is not mp.mpf:
t = mp.mpf("{:.15e}".format(t))
assert(t >= -epsilon)
assert(t <= self.duration + epsilon)
aVect = [curve.EvalAcc(t) for curve in self.curves]
return np.asarray(aVect)
def SetInitialValue(self, x0):
if type(x0) is not mp.mpf:
x0 = mp.mpf("{:.15e}".format(x0))
self.x0 = x0
newx0 = x0
for ramp in self.ramps:
ramp.x0 = newx0
newx0 = Add(newx0, ramp.d)
def compress(self, time_series: List[int]) -> int:
if time_series is None or len(time_series) == 0:
return 0
one_letter_probability = mp.mpf(1.0) / (self._alphabet_max_symbol -
self._alphabet_min_symbol + 1)
series_probability = mp.power(one_letter_probability, len(time_series))
return int(mp.ceil(-mp.log(series_probability, 2)))
def EvalAcc(self, t):
if type(t) is not mp.mpf:
t = mp.mpf("{:.15e}".format(t))
assert(t >= -epsilon)
assert(t <= self.duration + epsilon)
i, remainder = self._FindRampIndex(t)
return self.ramps[i].EvalAcc(remainder)
def UpdateDuration(self, newDur):
if type(newDur) is not mp.mpf:
newDur = mp.mpf("{:.15e}".format(newDur))
assert(newDur >= -epsilon)
self.duration = newDur
self.v1 = Add(self.v0, Mul(self.a, self.duration))
self.d = Prod([pointfive, Add(self.v0, self.v1), self.duration])
def EvalPos(self, t):
if type(t) is not mp.mpf:
t = mp.mpf("{:.15e}".format(t))
assert(t >= -epsilon)
assert(t <= self.duration + epsilon)
d_incr = Mul(t, Add(self.v0, Prod([pointfive, t, self.a])))
return Add(self.x0, d_incr)
def _gwr_no_memo(fn: Callable[[float], Any], time: float, M: int = 32, precin: int = 0) -> float:
"""
GWR alorithm without memoization. This is a near 1:1 translation from
Mathematica.
"""
tau = mp.log(2.0) / mp.mpf(time)
fni: List[float] = [0.0] * M
for i, n in enumerate(fni):
if i == 0:
continue
fni[i] = fn(n * tau)
G0: List[float] = [0.0] * M
Gp: List[float] = [0.0] * M
M1 = M
for n in range(1, M + 1):
try:
n_fac = mp.fac(n - 1)
G0[n - 1] = tau * mp.fac(2 * n) / (n * n_fac * n_fac)
s = 0.0
for i in range(n + 1):
s += mp.binomial(n, i) * (-1) ** i * fni[n + i]
G0[n - 1] *= s
except:
M1 = n - 1
break
best = G0[M1 - 1]
Gm: List[float] = [0.0] * M1
broken = False
for k in range(M1 - 1):
for n in range(M1 - 1 - k)[::-1]:
try:
expr = G0[n + 1] - G0[n]
except:
expr = 0.0
broken = True
break
expr = Gm[n + 1] + (k + 1) / expr
Gp[n] = expr
if k % 2 == 1 and n == M1 - 2 - k:
best = expr
if broken:
break
for n in range(M1 - k):
Gm[n] = G0[n]
G0[n] = Gp[n]
return best
def two_body_reference(self, t1, num_1 = 0, num_2 = 1):
""" reference notations are same as Yoel's notes
using a precision of 64 digits for max accuracy """
mp.dps = 64
self.load_data()
t1 = mp.mpf(t1)
m_1 = mp.mpf(self.planet_lst[num_1].mass)
m_2 = mp.mpf(self.planet_lst[num_2].mass)
x1_0 = mp.matrix(self.planet_lst[num_1].loc)
x2_0 = mp.matrix(self.planet_lst[num_2].loc)
v1_0 = mp.matrix(self.planet_lst[num_1].v)
v2_0 = mp.matrix(self.planet_lst[num_2].v)
M = m_1 + m_2
x_cm = (1/M)*((m_1*x1_0)+(m_2*x2_0))
v_cm = (1/M)*((m_1*v1_0)+(m_2*v2_0))
u1_0 = v1_0 - v_cm
w = x1_0 - x_cm
r_0 = mp.norm(w)
alpha = mp.acos((np.inner(u1_0,w))/(mp.norm(u1_0)*mp.norm(w)))
K = mp.mpf(self.G) * (m_2**3)/(M**2)
u1_0 = mp.norm(u1_0)
L = r_0 * u1_0 * mp.sin(alpha)
cosgamma = ((L**2)/(K*r_0)) - 1
singamma = -((L*u1_0*mp.cos(alpha))/K)
gamma = mp.atan2(singamma, cosgamma)
e = mp.sqrt(((r_0*(u1_0**2)*(mp.sin(alpha)**2)/K)-1)**2 + \
(((r_0**2)*(u1_0**4)*(mp.sin(alpha)**2)*(mp.cos(alpha)**2))/(K**2)) )
r_theta = lambda theta: (L**2)/(K*(1+(e*mp.cos(theta - gamma))))
f = lambda x: r_theta(x)**2/L
curr_theta = 0
max_it = 30
it = 1
converged = 0
while ((it < max_it) & (converged != 1)):
t_val = mp.quad(f,[0,curr_theta]) - t1
dt_val = f(curr_theta)
delta = t_val/dt_val
if (abs(delta)<1.0e-6):
converged = 1
curr_theta -= delta
it += 1
x1_new = mp.matrix([r_theta(curr_theta)*mp.cos(curr_theta), r_theta(curr_theta)*mp.sin(curr_theta)])
# x2_new = -(m_1/m_2)*(x1_new) +(x_cm + v_cm*t1)
x1_new = x1_new + (x_cm + v_cm*t1)
return x1_new
def geometric_mean(prices):
for price in prices:
if not isinstance(price, (int, long, float)):
raise TypeError('Prices must be a valid number.')
# We're using the mp module here to avoid the python error
# "OverflowError: long int too large to convert to float"
# See http://stackoverflow.com/a/29866503
return reduce(lambda x, y: x * y, prices) ** mp.mpf(1.0 / len(prices))
def RSZ_upto_c1(x):
x = mp.mpf(x.real)
tau = mp.sqrt(x/(2*PI))
N = int(tau.real)
p = tau-N
running_sum=0
for n in range(1,N+1):
running_sum += mp.cos(RStheta(x) - x*mp.log(n))/mp.sqrt(n)
return (2*running_sum + mp.power(-1,N-1)*mp.power(tau,-0.5)*(c0(p) - (1/tau)*c1(p))).real
def L_transform(j1,j2):
d = mp.mpf(10**(-mp.dps+1))
z0,z1 = j1*j2+1/(j1*j2), j1/j2+j2/j1
jp = d + mp.sqrt(z1)/mp.sqrt(z0)
dF = mp.log(mp.sqrt(z1)*mp.sqrt(z0))
return jp,dF
def error(coefs, progress=True):
(a, b) = coefs
xs = (x * mp.pi / mp.mpf(4096) for x in range(-4096, 4097))
err = max(fabs(sin(x) - f(x, a, b)) for x in xs)
if progress:
print('(a, b, c): ({}, {}, {})'.format(a, b, c(a, b)))
print('evaluated error: ', err)
print()
return float(err)
def calculate_pi(n):
percent = n / 100
acc = mp.mpf(0)
print(type(acc))
for i in range(n):
if i % percent == 0:
print(chr(0x25A9), sep="", end="")
acc += 4.0 * (1 - (i % 2) * 2) / (2 * i + 1)
return acc
def InterpolateZeroVelND(x0Vect, x1Vect, vmVect, amVect, delta=zero):
"""Interpolate a trajectory connecting two waypoints, x0Vect and x1Vect. Velocities at both
waypoints are zeros.
"""
ndof = len(x0Vect)
assert(ndof == len(x1Vect))
assert(ndof == len(vmVect))
assert(ndof == len(amVect))
# Convert all vector elements into mp.mpf (if necessary)
x0Vect_ = ConvertFloatArrayToMPF(x0Vect)
x1Vect_ = ConvertFloatArrayToMPF(x1Vect)
vmVect_ = ConvertFloatArrayToMPF(vmVect)
amVect_ = ConvertFloatArrayToMPF(amVect)
dVect = x1Vect - x0Vect
if type(delta) is not mp.mpf:
delta = mp.mpf("{:.15e}".format(delta))
vMin = inf # the tightest velocity bound
aMin = inf # the tightest acceleration bound
for i in range(ndof):
if not IsEqual(x1Vect[i], x0Vect[i]):
vMin = min(vMin, vmVect[i]/Abs(dVect[i]))
aMin = min(aMin, amVect[i]/Abs(dVect[i]))
if (not (vMin < inf and aMin < inf)):
# dVect is zero.
curvesnd = ParabolicCurvesND()
curvesnd.SetConstant(x0Vect_, 0)
return curvesnd
if delta == zero:
sdProfile = Interpolate1D(zero, one, zero, zero, vMin, aMin) # parabolic ramp (velocity profile sd(t))
else:
# Not handle interpolation with switch-time constraints yet
raise NotImplementedError
# Scale each DOF according to the obtained sd-profile
curves = [ParabolicCurve() for _ in range(ndof)] # a list of (empty) parabolic curves
for sdRamp in sdProfile:
aVect = sdRamp.a * dVect
v0Vect = sdRamp.v0 * dVect
dur = sdRamp.duration
for j in range(ndof):
ramp = Ramp(v0Vect[j], aVect[j], dur, x0Vect[j])
curve = ParabolicCurve([ramp])
curves[j].Append(curve)
for (i, curve) in enumerate(curves):
curve.SetInitialValue(x0Vect[i])
curvesnd = ParabolicCurvesND(curves)
return curvesnd
def rmnNnormsqr_Taylor(n, k, R, rmnBpol, rmnPpol):
#compute norm of spherical wave represented by A_2mn * (kr)^{n-1}*rmnBvec(kr) + A_3mn * (kr)^{n-1}*rmnPvec(kr)
#no conjugates taken since all the Arnoldi vectors will be purely real
kR = mp.mpf(k * R)
prefactpow, normpol = rmnNpol_dot(2 * n - 2, rmnBpol, rmnPpol, rmnBpol,
rmnPpol)
# print(normpol)
#print(po.polyval(k*R, normpol))
return (kR)**prefactpow * mp.re(po.polyval(kR, normpol)) / k**3
def chi(n, y):
if (n == 0):
return mp.mpf(1)
elif (n == 1):
return 2 * mp.sqrt(-mp.powm1(y, 2)) / (2 - mp.powm1(y, 2))
elif (n == 2):
return -mp.powm1(y, 2) / (2 * (2 - mod.powm1(y, 2)))
else:
return 0
def _operator_probability(cls, p_i, n_i, p_y, n_y):
"""
Return the probability of the given operator conditioned by the given probabilities.
NOTE: Assumes operator consists only of I and Y.
:param p_i: Probability of I
:type p_i: float
:param n_i: Number of I
:type n_i: int
:param p_y: Probability of Y
:type p_y: float
:param n_y: Number of Y
:type n_y: int
:return: All-Y operator probability
:rtype: mp.mpf
"""
return mp.mpf(p_y)**n_y * mp.mpf(p_i)**n_i
def chi(n, y):
if (n == 0):
return mp.mpf(1)
elif (n == 1):
return 2 * mp.sqrt(-mp.powm1(y, 2)) / keff_factor(y, cancel_keff=False)
elif (n == 2):
return -mp.powm1(y, 2) / (2 * keff_factor(y, cancel_keff=False))
else:
return 0
def UpdateDuration(self, newDur):
if type(newDur) is not mp.mpf:
newDur = mp.mpf("{:.15e}".format(newDur))
assert(newDur >= -epsilon)
self.duration = newDur
self.v1 = Add(self.v0, Mul(self.a, self.duration))
self.d = Prod([pointfive, Add(self.v0, self.v1), self.duration])
self.x1 = Add(self.x0, self.d)
def diff_cos_coeffs(an, jn, use_mp, scale):
r"""Compute coefficients of the derivative of a cosine series.
Similar as diff_sin_coeffs(), but for a function represented as cosine
series and with coefficients of the derivative as a sine series as result.
"""
if use_mp:
scale = mp.mpf(scale) # forces mpmath computation below
return [(-scale * j) * a for j, a in zip(jn, an)]
def RSZ_plain(x):
x = mp.mpf(x.real)
tau = mp.sqrt(x / (2 * mp.pi()))
N = int(tau.real)
z = 2 * (x - N) - 1
running_sum = 0
for n in range(1, N + 1):
running_sum += mp.cos(RStheta(x) - (x * mp.log(n))) / mp.sqrt(n)
return (2 * running_sum).real
def InterpolateZeroVelND(x0Vect, x1Vect, vmVect, amVect, delta=zero):
"""Interpolate a trajectory connecting two waypoints, x0Vect and x1Vect. Velocities at both
waypoints are zeros.
"""
ndof = len(x0Vect)
assert (ndof == len(x1Vect))
assert (ndof == len(vmVect))
assert (ndof == len(amVect))
# Convert all vector elements into mp.mpf (if necessary)
x0Vect_ = ConvertFloatArrayToMPF(x0Vect)
x1Vect_ = ConvertFloatArrayToMPF(x1Vect)
vmVect_ = ConvertFloatArrayToMPF(vmVect)
amVect_ = ConvertFloatArrayToMPF(amVect)
dVect = x1Vect - x0Vect
if type(delta) is not mp.mpf:
delta = mp.mpf("{:.15e}".format(delta))
vMin = inf # the tightest velocity bound
aMin = inf # the tightest acceleration bound
for i in xrange(ndof):
if not IsEqual(x1Vect[i], x0Vect[i]):
vMin = min(vMin, vmVect[i] / Abs(dVect[i]))
aMin = min(aMin, amVect[i] / Abs(dVect[i]))
assert (vMin < inf)
assert (aMin < inf)
if delta == zero:
sdProfile = Interpolate1D(
zero, one, zero, zero, vMin,
aMin) # parabolic ramp (velocity profile sd(t))
else:
# Not handle interpolation with switch-time constraints yet
raise NotImplementedError
# Scale each DOF according to the obtained sd-profile
curves = [ParabolicCurve()
for _ in xrange(ndof)] # a list of (empty) parabolic curves
for sdRamp in sdProfile:
aVect = sdRamp.a * dVect
v0Vect = sdRamp.v0 * dVect
dur = sdRamp.duration
for j in xrange(ndof):
ramp = Ramp(v0Vect[j], aVect[j], dur, x0Vect[j])
curve = ParabolicCurve([ramp])
curves[j].Append(curve)
for (i, curve) in enumerate(curves):
curve.SetInitialValue(x0Vect[i])
curvesnd = ParabolicCurvesND(curves)
return curvesnd
def SetInitialValue(self, x0):
if type(x0) is not mp.mpf:
x0 = mp.mpf("{:.15e}".format(x0))
self.x0 = x0
newx0 = x0
for ramp in self.ramps:
ramp.x0 = newx0
newx0 = Add(newx0, ramp.d)
self.x1 = Add(self.x0, self.d)
def _create_Tderiv(self, k, n):
r"""Create the n'th derivative of the k'th Chebyshev polynomial."""
if not self._use_mp:
return ChebyshevT.basis(k).deriv(n)
else:
if n == 0:
return lambda x: mp.chebyt(k, x)
# Since the Chebyshev derivatives attain high values near the
# borders +-1 and usually are subtracted to obtain small values,
# minimizing their relative error (i.e. fixing the number of
# correct decimal places (dps)) is not enough to get precise
# results. Hence, the below `moredps` variable is set to higher
# values close to the border.
extradps = 5
pos = mp.fprod(
[mp.mpf(k**2 - p**2) / (2 * p + 1) for p in range(n)])
neg = (-1)**(k + n) * pos
if n == 1:
def dTk(x):
with mp.extradps(extradps):
x = clip(x, -1.0, 1.0)
if mp.almosteq(x, mp.one): return pos
if mp.almosteq(x, -mp.one): return neg
moredps = max(
0,
int(-math.log10(
min(abs(x - mp.one), abs(x + mp.one))) / 2))
moredps = min(moredps, 100)
with mp.extradps(moredps):
t = mp.acos(x)
return k * mp.sin(k * t) / mp.sin(t)
return dTk
if n == 2:
def ddTk(x):
with mp.extradps(extradps):
x = clip(x, -1.0, 1.0)
if mp.almosteq(x, mp.one): return pos
if mp.almosteq(x, -mp.one): return neg
moredps = max(
0,
int(-math.log10(
min(abs(x - mp.one), abs(x + mp.one))) * 1.5) +
2)
moredps = min(moredps, 100)
with mp.extradps(moredps):
t = mp.acos(x)
s = mp.sin(t)
return -k**2 * mp.cos(k * t) / s**2 + k * mp.cos(
t) * mp.sin(k * t) / s**3
return ddTk
raise NotImplementedError(
'Derivatives of order > 2 not implemented.')
def _ImposeVelocityLimit(curve, vm):
"""_ImposeVelocityLimit imposes the given velocity limit to the ParabolicCurve. In case the velocity
limit cannot be satisfied, this function will return an empty ParabolicCurve.
"""
# Check types
if type(vm) is not mp.mpf:
vm = mp.mpf("{:.15e}".format(vm))
# Check inputs
assert (vm > zero)
assert (len(curve) == 2)
assert (Add(curve[0].a, curve[1].a) == zero)
if Sub(Abs(curve[0].v0), vm) > epsilon:
# Initial velocity violates the constraint
return ParabolicCurve()
if Sub(Abs(curve[1].v1), vm) > epsilon:
# Final velocity violates the constraint
return ParabolicCurve()
vp = curve[1].v0
if Abs(vp) <= vm:
# Velocity limit is not violated
return curve
# h = Sub(Abs(vp), vm)
# t = mp.fdiv(h, Abs(curve[0].a))
ramp0, ramp1 = curve
h = Sub(Abs(vp), vm)
t = mp.fdiv(h, Abs(ramp0.a))
# import IPython; IPython.embed()
ramps = []
if IsEqual(Abs(ramp0.v0), vm) and (mp.sign(ramp0.v0) == mp.sign(vp)):
assert (IsEqual(ramp0.duration, t)) # check soundness
else:
newRamp0 = Ramp(ramp0.v0, ramp0.a, Sub(ramp0.duration, t), ramp0.x0)
ramps.append(newRamp0)
nom = h**2
denom = Mul(Abs(curve[0].a), vm)
newRamp1 = Ramp(Mul(mp.sign(vp), vm), zero,
Sum([t, t, mp.fdiv(nom, denom)]), curve.x0)
ramps.append(newRamp1)
if IsEqual(Abs(ramp1.v1), vm) and (mp.sign(ramp1.v1) == mp.sign(vp)):
assert (IsEqual(ramp1.duration, t)) # check soundness
else:
newRamp2 = Ramp(Mul(mp.sign(vp), vm), ramp1.a, Sub(ramp1.duration, t))
ramps.append(newRamp2)
return ParabolicCurve(ramps)
def _error_estimate1(
h,
sinh_t,
cosh_t,
cosh_sinh_t,
y0,
y1,
fly,
fry,
f_left,
f_right,
alpha,
last_estimate,
):
"""
A pretty accurate error estimation is
E(h) = h * (h/2/pi)**2 * sum_{-N}^{+N} F''(h*j)
with
F(t) = f(g(t)) * g'(t),
g(t) = tanh(pi/2 sinh(t)).
"""
alpha2 = alpha / mp.mpf(2)
sinh_sinh_t = numpy.array(list(map(mp.sinh, sinh_t)))
tanh_sinh_t = sinh_sinh_t / cosh_sinh_t
# More derivatives of y = 1-g(t).
y2 = -alpha2 * (sinh_t - 2 * cosh_t**2 * tanh_sinh_t) / cosh_sinh_t**2
y3 = (-alpha2 * cosh_t *
(+cosh_sinh_t - 4 * cosh_t**2 / cosh_sinh_t + 2 * cosh_t**2 *
cosh_sinh_t + 2 * cosh_t**2 * tanh_sinh_t * sinh_sinh_t -
6 * sinh_t * sinh_sinh_t) / cosh_sinh_t**3)
fl1_y = numpy.array([f_left[1](yy) for yy in y0])
fl2_y = numpy.array([f_left[2](yy) for yy in y0])
fr1_y = numpy.array([f_right[1](yy) for yy in y0])
fr2_y = numpy.array([f_right[2](yy) for yy in y0])
# Second derivative of F(t) = f(g(t)) * g'(t).
summands = numpy.concatenate([
y3 * fly + 3 * y1 * y2 * fl1_y + y1**3 * fl2_y,
y3 * fry + 3 * y1 * y2 * fr1_y + y1**3 * fr2_y,
])
val = h * (h / 2 / mp.pi)**2 * mp.fsum(summands)
if last_estimate is None:
# Root level: The midpoint is counted twice in the above sum.
out = val / 2
else:
out = last_estimate / 8 + val
return out
def cMLE(self, ip, *args):
"""
Estimation of c
"""
c = float(ip)
a = float(self.arule[self.j + 1])
b = float(self.brule[self.j + 1])
#print("a = {}, b = {}, c = {}".format(a, b, c))
tVec = self.tVec
tn = tVec[-1]
exp_tn = np.exp(-b * np.power(tn, c))
n = np.size(tVec)
sum_k = 0
for i in range(n):
if i > 0:
numer = (np.power(self.tVec[i], c) * self.expo(i, b, c) *
np.log(tVec[i]) - np.power(self.tVec[i - 1], c) *
self.expo(i - 1, b, c) * np.log(tVec[i - 1]))
denom = (self.expo(i - 1, b, c) - self.expo(i, b, c))
else:
numer = (np.power(self.tVec[i], c) * self.expo(i, b, c) *
np.log(tVec[i]))
denom = (1 - self.expo(i, b, c))
if (denom == 0 or numer == 0) and i > 0:
denom = exp(-mp.mpf(str(b)) * power(str(
self.tVec[i - 1]), mp.mpf(str(c)))) - exp(
-mp.mpf(str(b)) *
power(str(self.tVec[i]), mp.mpf(str(c))))
numer = (
power(float(self.tVec[i]), c) * self.expo_mp(i, b, c) *
log(float(self.tVec[i])) -
power(float(self.tVec[i - 1]), c) *
self.expo_mp(i - 1, b, c) * log(float(self.tVec[i - 1])))
#print ("{} / {}".format(type(numer),type(denom)))
ratio = numer / denom
#print(ratio)
sum_k += self.kVec[i] * b * float(ratio)
cprime = sum_k - a * b * np.power(self.tVec[-1], c) * self.expo(
-1, b, c) * np.log(tVec[-1])
return float(cprime)
def gauss_genlaguerre(n, alpha):
d = mp.matrix([i + alpha for i in mp.arange(1, 2 * n, 2)])
e = [-mp.sqrt(k * (k + alpha)) for k in mp.arange(1, n)]
e.append(mp.mpf('0.0'))
e = mp.matrix(e)
z = mp.eye(n)[0, :]
tridiag_eigen(mp, d, e, z)
z = mp.gamma(alpha + 1) * z.apply(lambda x: x**2)
return d, z.T
def binaryToDecFaster(binaryInitial):
with Pool(8) as p:
tt = p.map(binaryReducer, enumerate(binaryInitial))
res = mp.mpf(0)
for _ in tt:
res += _
return res
def __init__(self,
numerator,
denominator,
n_elements=3,
precision=4,
max_numerator=100,
max_denominator=100):
mp.dps = 10
self.numerator = numerator
self.denominator = denominator
self.fraction = Fraction(numerator, denominator)
self.precision = precision
self.combinations_checked = 0
self.fractions = []
self.n_elements = n_elements
self.ratio = mp.mpf(numerator) / mp.mpf(denominator)
self.max_denominator = max_denominator
self.max_numerator = max_numerator
self.total_elements = 0
def _ImposeVelocityLimit(curve, vm):
"""_ImposeVelocityLimit imposes the given velocity limit to the ParabolicCurve. In case the velocity
limit cannot be satisfied, this function will return an empty ParabolicCurve.
"""
# Check types
if type(vm) is not mp.mpf:
vm = mp.mpf("{:.15e}".format(vm))
# Check inputs
assert(vm > zero)
assert(len(curve) == 2)
assert(Add(curve[0].a, curve[1].a) == zero)
if Sub(Abs(curve[0].v0), vm) > epsilon:
# Initial velocity violates the constraint
return ParabolicCurve()
if Sub(Abs(curve[1].v1), vm) > epsilon:
# Final velocity violates the constraint
return ParabolicCurve()
vp = curve[1].v0
if Abs(vp) <= vm:
# Velocity limit is not violated
return curve
# h = Sub(Abs(vp), vm)
# t = mp.fdiv(h, Abs(curve[0].a))
ramp0, ramp1 = curve
h = Sub(Abs(vp), vm)
t = mp.fdiv(h, Abs(ramp0.a))
# import IPython; IPython.embed()
ramps = []
if IsEqual(Abs(ramp0.v0), vm) and (mp.sign(ramp0.v0) == mp.sign(vp)):
assert(IsEqual(ramp0.duration, t)) # check soundness
else:
newRamp0 = Ramp(ramp0.v0, ramp0.a, Sub(ramp0.duration, t), ramp0.x0)
ramps.append(newRamp0)
nom = h**2
denom = Mul(Abs(curve[0].a), vm)
newRamp1 = Ramp(Mul(mp.sign(vp), vm), zero, Sum([t, t, mp.fdiv(nom, denom)]), curve.x0)
ramps.append(newRamp1)
if IsEqual(Abs(ramp1.v1), vm) and (mp.sign(ramp1.v1) == mp.sign(vp)):
assert(IsEqual(ramp1.duration, t)) # check soundness
else:
newRamp2 = Ramp(Mul(mp.sign(vp), vm), ramp1.a, Sub(ramp1.duration, t))
ramps.append(newRamp2)
return ParabolicCurve(ramps)
def InterpolateArbitraryVelND(x0Vect_, x1Vect_, v0Vect_, v1Vect_, xminVect_, xmaxVect_, vmVect_, amVect_, delta=zero, tryHarder=False):
"""Interpolate a trajectory connecting two waypoints, (x0Vect, v0Vect) and (x1Vect, v1Vect).
"""
ndof = len(x0Vect_)
assert(ndof == len(x1Vect_))
assert(ndof == len(v0Vect_))
assert(ndof == len(v1Vect_))
assert(ndof == len(xminVect_))
assert(ndof == len(xmaxVect_))
assert(ndof == len(vmVect_))
assert(ndof == len(amVect_))
# Convert all vector elements into mp.mpf (if necessary)
x0Vect = ConvertFloatArrayToMPF(x0Vect_)
x1Vect = ConvertFloatArrayToMPF(x1Vect_)
v0Vect = ConvertFloatArrayToMPF(v0Vect_)
v1Vect = ConvertFloatArrayToMPF(v1Vect_)
xminVect = ConvertFloatArrayToMPF(xminVect_)
xmaxVect = ConvertFloatArrayToMPF(xmaxVect_)
vmVect = ConvertFloatArrayToMPF(vmVect_)
amVect = ConvertFloatArrayToMPF(amVect_)
dVect = x1Vect - x0Vect
if type(delta) is not mp.mpf:
delta = mp.mpf("{:.15e}".format(delta))
# First independently interpolate each DOF to find out the slowest one.
curves = []
maxDuration = zero
maxIndex = 0
for i in range(ndof):
if delta == zero:
curve = Interpolate1D(x0Vect[i], x1Vect[i], v0Vect[i], v1Vect[i], vmVect[i], amVect[i])
else:
raise NotImplementedError
curves.append(curve)
if curve.duration > maxDuration:
maxDuration = curve.duration
maxIndex = i
## TEMPORARY
# print("maxIndex = {0}".format(maxIndex))
curvesnd = ReinterpolateNDFixedDuration(curves, vmVect, amVect, maxIndex, delta, tryHarder)
newCurves = []
for (i, curve) in enumerate(curvesnd.curves):
newCurve = _ImposeJointLimitFixedDuration(curve, xminVect[i], xmaxVect[i], vmVect[i], amVect[i])
if newCurve.isEmpty:
return ParabolicCurvesND()
newCurves.append(newCurve)
return ParabolicCurvesND(newCurves)
def __init__(self, v0, a, dur, x0=zero):
if type(dur) is not mp.mpf:
dur = mp.mpf("{:.15e}".format(dur))
assert(dur >= -epsilon)
# Check types
if type(x0) is not mp.mpf:
x0 = mp.mpf("{:.15e}".format(x0))
if type(v0) is not mp.mpf:
v0 = mp.mpf("{:.15e}".format(v0))
if type(a) is not mp.mpf:
a = mp.mpf("{:.15e}".format(a))
self.x0 = x0
self.v0 = v0
self.a = a
self.duration = dur
self.v1 = Add(self.v0, Mul(self.a, self.duration))
self.d = Prod([pointfive, Add(self.v0, self.v1), self.duration])
def test_levin_1():
mp.dps = 17
eps = mp.mpf(mp.eps)
with mp.extraprec(2 * mp.prec):
L = mp.levin(method = "levin", variant = "v")
A, n = [], 1
while 1:
s = mp.mpf(n) ** (2 + 3j)
n += 1
A.append(s)
v, e = L.update(A)
if e < eps:
break
if n > 1000: raise RuntimeError("iteration limit exceeded")
eps = mp.exp(0.9 * mp.log(eps))
err = abs(v - mp.zeta(-2-3j))
assert err < eps
w = mp.nsum(lambda n: n ** (2 + 3j), [1, mp.inf], method = "levin", levin_variant = "v")
err = abs(v - w)
assert err < eps
def MPDBsc(M):
Xk = M.copy()
Yk = mp.eye(M.cols)
dA = mp.det(M)**(mp.mpf('1.0')/2)
for i in range(1,30):
uk = mp.fabs(mp.det(Xk)/dA)**(-1.0/i)
Xk1 = (uk*Xk + (uk**(-1))*(Yk**(-1)))/2
Yk1 = (uk*Yk + (uk**(-1))*(Xk**(-1)))/2
Xk = Xk1
Yk = Yk1
return Xk
def Interpolate1D(x0, x1, v0, v1, vm, am, delta=zero):
# Check types
if type(x0) is not mp.mpf:
x0 = mp.mpf("{:.15e}".format(x0))
if type(x1) is not mp.mpf:
x1 = mp.mpf("{:.15e}".format(x1))
if type(v0) is not mp.mpf:
v0 = mp.mpf("{:.15e}".format(v0))
if type(v1) is not mp.mpf:
v1 = mp.mpf("{:.15e}".format(v1))
if type(vm) is not mp.mpf:
vm = mp.mpf("{:.15e}".format(vm))
if type(am) is not mp.mpf:
am = mp.mpf("{:.15e}".format(am))
# Check inputs
assert(vm > zero)
assert(am > zero)
assert(Abs(v0) <= vm)
assert(Abs(v1) <= vm)
curve = _Interpolate1DNoVelocityLimit(x0, x1, v0, v1, am)
if len(curve) == 1:
return curve
return _ImposeVelocityLimit(curve, vm)
def test_levin_3():
mp.dps = 17
z=mp.mpf(2)
eps = mp.mpf(mp.eps)
with mp.extraprec(7*mp.prec): # we need copious amount of precision to sum this highly divergent series
L = mp.levin(method = "levin", variant = "t")
n, s = 0, 0
while 1:
s += (-z)**n * mp.fac(4 * n) / (mp.fac(n) * mp.fac(2 * n) * (4 ** n))
n += 1
v, e = L.step_psum(s)
if e < eps:
break
if n > 1000: raise RuntimeError("iteration limit exceeded")
eps = mp.exp(0.8 * mp.log(eps))
exact = mp.quad(lambda x: mp.exp( -x * x / 2 - z * x ** 4), [0,mp.inf]) * 2 / mp.sqrt(2 * mp.pi)
# there is also a symbolic expression for the integral:
# exact = mp.exp(mp.one / (32 * z)) * mp.besselk(mp.one / 4, mp.one / (32 * z)) / (4 * mp.sqrt(z * mp.pi))
err = abs(v - exact)
assert err < eps
w = mp.nsum(lambda n: (-z)**n * mp.fac(4 * n) / (mp.fac(n) * mp.fac(2 * n) * (4 ** n)), [0, mp.inf], method = "levin", levin_variant = "t", workprec = 8*mp.prec, steps = [2] + [1 for x in xrange(1000)])
err = abs(v - w)
assert err < eps
def test_levin_2():
# [2] A. Sidi - "Pratical Extrapolation Methods" p.373
mp.dps = 17
z=mp.mpf(10)
eps = mp.mpf(mp.eps)
with mp.extraprec(2 * mp.prec):
L = mp.levin(method = "sidi", variant = "t")
n = 0
while 1:
s = (-1)**n * mp.fac(n) * z ** (-n)
v, e = L.step(s)
n += 1
if e < eps:
break
if n > 1000: raise RuntimeError("iteration limit exceeded")
eps = mp.exp(0.9 * mp.log(eps))
exact = mp.quad(lambda x: mp.exp(-x)/(1+x/z),[0,mp.inf])
# there is also a symbolic expression for the integral:
# exact = z * mp.exp(z) * mp.expint(1,z)
err = abs(v - exact)
assert err < eps
w = mp.nsum(lambda n: (-1) ** n * mp.fac(n) * z ** (-n), [0, mp.inf], method = "sidi", levin_variant = "t")
assert err < eps
def SolveQuartic(a, b, c, d, e):
"""
SolveQuartic solves a quartic (fouth order) equation of the form
ax^4 + bx^3 + cx^2 + dx + e = 0.
For the detail of formulae presented here, see https://en.wikipedia.org/wiki/Quartic_function
"""
# Check types
if type(a) is not mp.mpf:
a = mp.mpf("{:.15e}".format(a))
if type(b) is not mp.mpf:
b = mp.mpf("{:.15e}".format(b))
if type(c) is not mp.mpf:
c = mp.mpf("{:.15e}".format(c))
if type(d) is not mp.mpf:
d = mp.mpf("{:.15e}".format(d))
if type(e) is not mp.mpf:
e = mp.mpf("{:.15e}".format(e))
"""
# Working code (more readable but probably less precise)
p = (8*a*c - 3*b*b)/(8*a*a)
q = (b**3 - 4*a*b*c + 8*a*a*d)/(8*a*a*a)
delta0 = c*c - 3*b*d + 12*a*e
delta1 = 2*(c**3) - 9*b*c*d + 27*b*b*e + 27*a*d*d - 72*a*c*e
Q = mp.nthroot(pointfive*(delta1 + mp.sqrt(delta1*delta1 - 4*mp.power(delta0, 3))), 3)
S = pointfive*mp.sqrt(-mp.fdiv(mp.mpf('2'), mp.mpf('3'))*p + (one/(3*a))*(Q + delta0/Q))
x1 = -b/(4*a) - S + pointfive*mp.sqrt(-4*S*S - 2*p + q/S)
x2 = -b/(4*a) - S - pointfive*mp.sqrt(-4*S*S - 2*p + q/S)
x3 = -b/(4*a) + S + pointfive*mp.sqrt(-4*S*S - 2*p - q/S)
x4 = -b/(4*a) + S - pointfive*mp.sqrt(-4*S*S - 2*p - q/S)
"""
p = mp.fdiv(Sub(Prod([number('8'), a, c]), Mul(number('3'), mp.power(b, 2))), Mul(number('8'), mp.power(a, 2)))
q = mp.fdiv(Sum([mp.power(b, 3), Prod([number('-4'), a, b, c]), Prod([number('8'), mp.power(a, 2), d])]), Mul(8, mp.power(a, 3)))
delta0 = Sum([mp.power(c, 2), Prod([number('-3'), b, d]), Prod([number('12'), a, e])])
delta1 = Sum([Mul(2, mp.power(c, 3)), Prod([number('-9'), b, c, d]), Prod([number('27'), mp.power(b, 2), e]), Prod([number('27'), a, mp.power(d, 2)]), Prod([number('-72'), a, c, e])])
Q = mp.nthroot(Mul(pointfive, Add(delta1, mp.sqrt(Add(mp.power(delta1, 2), Mul(number('-4'), mp.power(delta0, 3)))))), 3)
S = Mul(pointfive, mp.sqrt(Mul(mp.fdiv(mp.mpf('-2'), mp.mpf('3')), p) + Mul(mp.fdiv(one, Mul(number('3'), a)), Add(Q, mp.fdiv(delta0, Q)))))
# log.debug("p = {0}".format(mp.nstr(p, n=_prec)))
# log.debug("q = {0}".format(mp.nstr(q, n=_prec)))
# log.debug("delta0 = {0}".format(mp.nstr(delta0, n=_prec)))
# log.debug("delta1 = {0}".format(mp.nstr(delta1, n=_prec)))
# log.debug("Q = {0}".format(mp.nstr(Q, n=_prec)))
# log.debug("S = {0}".format(mp.nstr(S, n=_prec)))
x1 = Sum([mp.fdiv(b, Mul(number('-4'), a)), Neg(S), Mul(pointfive, mp.sqrt(Sum([Mul(number('-4'), mp.power(S, 2)), Mul(number('-2'), p), mp.fdiv(q, S)])))])
x2 = Sum([mp.fdiv(b, Mul(number('-4'), a)), Neg(S), Neg(Mul(pointfive, mp.sqrt(Sum([Mul(number('-4'), mp.power(S, 2)), Mul(number('-2'), p), mp.fdiv(q, S)]))))])
x3 = Sum([mp.fdiv(b, Mul(number('-4'), a)), S, Mul(pointfive, mp.sqrt(Sum([Mul(number('-4'), mp.power(S, 2)), Mul(number('-2'), p), Neg(mp.fdiv(q, S))])))])
x4 = Sum([mp.fdiv(b, Mul(number('-4'), a)), S, Neg(Mul(pointfive, mp.sqrt(Sum([Mul(number('-4'), mp.power(S, 2)), Mul(number('-2'), p), Neg(mp.fdiv(q, S))]))))])
return [x1, x2, x3, x4]
def apply(self, n, b, evaluation):
'IntegerLength[n_, b_]'
# Use interval arithmetic to account for "right" rounding
n, b = n.get_int_value(), b.get_int_value()
if n is None or b is None:
evaluation.message('IntegerLength', 'int')
return
if b <= 1:
evaluation.message('IntegerLength', 'base', b)
return
result = mp.mpf(iv.log(iv.mpf(mpz2mpmath(abs(n))), mpz2mpmath(b)).b)
result = mpz(mpmath2gmpy(result)) + 1
return Integer(result)
