Exemple #1
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)
Exemple #2
0
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
Exemple #3
0
def _SolveAXMB(a, b, eps, xmin, xmax):
    """
    This function 'safely' solves for a value x \in [xmin, xmax] such that  |a*x - b| <= eps*max(|a|, |b|).
    Assume xmin <= 0 <= xmax.

    This function returns [result, x].
    """
    if (a < 0):
        return _SolveAXMB(-a, -b, eps, xmin, xmax)

    epsScaled = eps*max(a, Abs(b)) # we already know that a > 0

    # Infinite range
    if ((xmin == -inf) and (xmax == inf)):
        if (a == zero):
            x = zero
            result = Abs(b) <= epsScaled
            return [result, x]

        x = mp.fdiv(b, a)
        return [True, x]

    axmin = Mul(a, xmin)
    axmax = Mul(a, xmax)

    if not ((Add(b, epsScaled) >= axmin) and (Sub(b, epsScaled) <= axmax)):
        # Ranges do not intersect
        return [False, zero]

    if not (a == zero):
        x = mp.fdiv(b, a)
        if (xmin <= x) and (x <= xmax):
            return [True, x]

    if (Abs(Sub(Mul(pointfive, Add(axmin, axmax)), b)) <= epsScaled):
        x = Mul(pointfive, Add(xmin, xmax))
        return [True, x]

    if (Abs(Sub(axmax, b)) <= epsScaled):
        x = xmax
        return [True, x]

    assert(Abs(Sub(axmin, b)) <= epsScaled)
    x = xmin
    return [True, x]
Exemple #4
0
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]
Exemple #5
0
    def SetSegment(self, x0, x1, v0, v1, t):
        t = ConvertFloatToMPF(t)
        assert(t >= 0)
        x0 = ConvertFloatToMPF(x0)
        x1 = ConvertFloatToMPF(x1)
        v0 = ConvertFloatToMPF(v0)
        v1 = ConvertFloatToMPF(v1)

        if FuzzyZero(t, epsilon):
            a = 0
        else:
            tSqr = Sqr(t)
            a = mp.fdiv(Neg(Sum([Mul(v0, tSqr), Mul(t, Sub(x0, x1)), Mul(2, Sub(v0, v1))])), Mul(t, Add(Mul(pointfive, tSqr), 2)))
        ramp0 = Ramp(v0, a, t, x0)
        self.Initialize([ramp0])
        return
Exemple #6
0
def _CalculateLeastUpperBoundInoperativeInterval(x0, x1, v0, v1, vm, am):
    # All input must already be of mp.mpf type
    d = x1 - x0
    temp1 = Prod([number('2'), Neg(Sqr(am)), Sub(Prod([number('2'), am, d]), Add(Sqr(v0), Sqr(v1)))])
    if temp1 < zero:
        T0 = number('-1')
        T1 = number('-1')
    else:
        term1 = mp.fdiv(Add(v0, v1), am)
        term2 = mp.fdiv(mp.sqrt(temp1), Sqr(am))
        T0 = Add(term1, term2)
        T1 = Sub(term1, term2)

    temp2 = Prod([number('2'), Sqr(am), Add(Prod([number('2'), am, d]), Add(Sqr(v0), Sqr(v1)))])
    if temp2 < zero:
        T2 = number('-1')
        T3 = number('-1')
    else:
        term1 = Neg(mp.fdiv(Add(v0, v1), am))
        term2 = mp.fdiv(mp.sqrt(temp2), Sqr(am))
        T2 = Add(term1, term2)
        T3 = Sub(term1, term2)

    newDuration = max(max(T0, T1), max(T2, T3))
    if newDuration > zero:
        dStraight = Prod([pointfive, Add(v0, v1), newDuration])
        if Sub(d, dStraight) > 0:
            amNew = am
            vmNew = vm
        else:
            amNew = -am
            vmNew = -vm

        # import IPython; IPython.embed()

        vp = Mul(pointfive, Sum([Mul(newDuration, amNew), v0, v1])) # the peak velocity
        if (Abs(vp) > vm):
            dExcess = mp.fdiv(Sqr(Sub(vp, vmNew)), am)
            assert(dExcess > 0)
            deltaTime = mp.fdiv(dExcess, vm)
            newDuration = Add(newDuration, deltaTime)

        log.debug('Calculation successful: T0 = {0}; T1 = {1}; T2 = {2}; T3 = {3}'.format(mp.nstr(T0, n=_prec), mp.nstr(T1, n=_prec), mp.nstr(T2, n=_prec), mp.nstr(T3, n=_prec)))
        
        newDuration = Mul(newDuration, number('1.01')) # add 1% safety bound
        return newDuration
    else:
        if (FuzzyEquals(x0, x1, epsilon) and FuzzyZero(v0, epsilon) and FuzzyZero(v1, epsilon)):
            # t = 0 is actually a correct solution
            newDuration = 0
            return newDuration
        log.debug('Unable to calculate the least upper bound: T0 = {0}; T1 = {1}; T2 = {2}; T3 = {3}'.\
                  format(mp.nstr(T0, n=_prec), mp.nstr(T1, n=_prec), mp.nstr(T2, n=_prec), mp.nstr(T3, n=_prec)))
        return number('-1')
Exemple #7
0
    def _GetPeaks(self, ta, tb):
        if (ta > tb):
            return self._GetPeaks(tb, ta)

        if (ta < 0):
            ta = 0
        elif (ta >= self.duration):
            xmin = self.x1
            xmax = self.x1
            return [xmin, xmax]

        if (tb <= 0):
            xmin = self.x0
            xmax = self.x0
            return [xmin, xmax]
        elif (tb >= self.duration):
            tb = self.duration        
        
        if (FuzzyZero(self.a, epsilon)):
            if self.v0 > 0:
                xmin = self.EvalPos(ta)
                xmax = self.EvalPos(tb)
            else:
                xmin = self.EvalPos(tb)
                xmax = self.EvalPos(ta)
            return [xmin, xmax]

        tempa = self.EvalPos(ta)
        tempb = self.EvalPos(tb)
        if (tempa < tempb):
            xmin = tempa
            xmax = tempb
        else:
            xmin = tempb
            xmax = tempa
        
        tDeflection = Neg(mp.fdiv(self.v0, self.a))
        if (tDeflection <= ta) or (tDeflection >= tb):
            return [xmin, xmax]
        
        xDeflection = self.EvalPos(tDeflection)
        xmax = max(xmax, xDeflection)
        xmin = min(xmin, xDeflection)
        return [xmin, xmax]
Exemple #8
0
 def GetPeaks(self):
     if (FuzzyZero(self.a, epsilon)):
         if self.v0 > 0:
             xmin = self.x0
             xmax = self.EvalPos(self.duration)
         else:
             xmin = self.EvalPos(self.duration)
             xmax = self.x0
         return [xmin, xmax]
     elif (self.a > 0):
         xmin = self.x0
         xmax = self.EvalPos(self.duration)
     else:
         xmin = self.EvalPos(self.duration)
         xmax = self.x0            
         
     tDeflection = Neg(mp.fdiv(self.v0, self.a))
     if (tDeflection <= 0) or (tDeflection >= self.duration):
         return [xmin, xmax]
     
     xDeflection = self.EvalPos(tDeflection)
     xmax = max(xmax, xDeflection)
     xmin = min(xmin, xDeflection)
     return [xmin, xmax]
Exemple #9
0
def is_inverse_sum(numbers):
    """This function checks if the array of integers are an inverse sum of 0.5"""
    return mp.fsum([mp.fdiv(1, (n * n)) for n in numbers]) == 0.5
Exemple #10
0
    print "usage: composite [odd integer to be tested] [parameter for accuracy]"
    print "output with n-1 as 2^s * d: (witness, s, d)"
    sys.exit()

n = int(sys.argv[1]) # odd integer to be tested for primality
"""
s = int(sys.argv[2]) # n-1 as 2^s*d
d = mp.mpf(sys.argv[3])
"""
k = int(sys.argv[2]) # parameter that determines the accuracy of the test
s = 0
d = 0

# find s and d first
for i in range(0, 30):
    x = mp.fdiv((n-1), mp.power(2,i))
    if x - long(x) == 0:
        s = i
        d = int(x)

def composite():
    for i in range(0, k):
        a = random.randint(2,100)#n-2)
        x = mp.fmod(mp.power(a,d),n)
        if x != 1 and x != n-1:#
            comp = True
            for r in range(1, s):
                x = mp.fmod(mp.power(x,2),n)
                if x == 1:#
                    return a, s, d, r
                if x == n-1:#
def dipole_approx(r, theta, charge, a):
    '''
    Purpose      : To calculate Electric Potential due to Dipole neglecting very small value of a^2

    Formula Used :
            q*2*a*cos(theta)*const_k/(r**2)
                   where,
                    const_k = 4*pi*epsilon_not
                            = 8.9875518 x 10^9

    Parameters   :
                   a) r      - distance between center of the dipole and point of observation
                   b) theta  - Angle between positive charge and point of observation
                   c) charge - either charge irrespective of sign
                   d) a      - distance between either charge and center of dipole

    Return : returns approx electric field calculated
    '''
    '''
    Editing Values of the arguments as per the values received by this function
    '''
    # Editing value of r as per the unit
    if r[1].lower() in ('meters', 'm'):
        r = r[0]
    elif r[1].lower() in ('centimeters', 'cm'):
        r = mp.fmul(r[0], 10**(-2))
    elif r[1].lower() in ('millimeters', 'mm'):
        r = mp.fmul(r[0], 10**(-3))
    elif r[1].lower() in ('angstroms', 'a', 'A'):
        r = mp.fmul(r[0], 10**(-10))

    # Editing value of a as per the unit
    if a[1].lower() in ('meters', 'm'):
        a = a[0]
    elif a[1].lower() in ('centimeters', 'cm'):
        a = mp.fmul(a[0], 10**(-2))
    elif a[1].lower() in ('millimeters', 'mm'):
        a = mp.fmul(a[0], 10**(-3))
    elif a[1].lower() in ('angstroms', 'a', 'A'):
        a = mp.fmul(a[0], 10**(-10))

    # Calculating Value of Cos(theta)
    if theta[1].lower() == 'radians':
        cos_theta = round(cos(theta[0]), 5)
    elif theta[1].lower() == 'degrees':
        cos_theta = round(cos(mp.radians(theta[0])), 5)

    # Editing Value of charge as per the unit
    if charge[1] in ('Coulomb', 'C'):
        charge = charge[0]
    elif charge[1] in ('microCoulomb', 'uC'):
        charge = mp.fmul(charge[0], 10**(-6))
    elif charge[1] in ('milliCoulomb', 'mC'):
        charge = mp.fmul(charge[0], 10**(-3))
    elif charge[1] in ('electronCharge', 'eC'):
        charge = mp.fmul(charge[0], mp.fmul(1.60217646,
                                            mp.fmul(10,
                                                    -19)))  # 1.60217646⋅10-19
    elif charge[1] in ('nanoCoulomb', 'nC'):
        charge = mp.fmul(charge[0], mp.power(10, -10))
    elif charge[1] in ('picoCharge', 'pC'):
        charge = mp.fmul(charge[0], mp.power(10, -12))

    # Applying Formula to given parameters
    result = mp.fdiv(
        mp.fmul(const_k, mp.fmul(charge, mp.fmul(2, mp.fmul(a, cos_theta)))),
        mp.power(r, 2))

    # returning final result
    return result
Exemple #12
0
def Interpolate1DFixedDuration(x0, x1, v0, v1, newDuration, vm, am):
    x0 = ConvertFloatToMPF(x0)
    x1 = ConvertFloatToMPF(x1)
    v0 = ConvertFloatToMPF(v0)
    v1 = ConvertFloatToMPF(v1)
    vm = ConvertFloatToMPF(vm)
    am = ConvertFloatToMPF(am)
    newDuration = ConvertFloatToMPF(newDuration)
    log.debug("\nx0 = {0}; x1 = {1}; v0 = {2}; v1 = {3}; vm = {4}; am = {5}; newDuration = {6}".\
              format(mp.nstr(x0, n=_prec), mp.nstr(x1, n=_prec), mp.nstr(v0, n=_prec), mp.nstr(v1, n=_prec),
                     mp.nstr(vm, n=_prec), mp.nstr(am, n=_prec), mp.nstr(newDuration, n=_prec)))
    
    # Check inputs
    assert(vm > zero)
    assert(am > zero)

    if (newDuration < -epsilon):
        log.info("duration = {0} is negative".format(newDuration))
        return ParabolicCurve()
    if (newDuration <= epsilon):
        # Check if this is a stationary trajectory
        if (FuzzyEquals(x0, x1, epsilon) and FuzzyEquals(v0, v1, epsilon)):
            log.info("stationary trajectory")
            ramp0 = Ramp(v0, 0, 0, x0)
            newCurve = ParabolicCurve(ramp0)
            return newCurve
        else:
            log.info("newDuration is too short for any movement to be made")
            return ParabolicCurve()

    # Correct small discrepancies if any
    if (v0 > vm):
        if FuzzyEquals(v0, vm, epsilon):
            v0 = vm
        else:
            log.info("v0 > vm: {0} > {1}".format(v0, vm))
            return ParabolicCurve()
    elif (v0 < -vm):
        if FuzzyEquals(v0, -vm, epsilon):
            v0 = -vm
        else:
            log.info("v0 < -vm: {0} < {1}".format(v0, -vm))
            return ParabolicCurve()
    if (v1 > vm):
        if FuzzyEquals(v1, vm, epsilon):
            v1 = vm
        else:
            log.info("v1 > vm: {0} > {1}".format(v1, vm))
            return ParabolicCurve()
    elif (v1 < -vm):
        if FuzzyEquals(v1, -vm, epsilon):
            v1 = -vm
        else:
            log.info("v1 < -vm: {0} < {1}".format(v1, -vm))
            return ParabolicCurve()

    d = Sub(x1, x0)

    # First assume no velocity bound -> re-interpolated trajectory will have only two ramps.
    # Solve for a0 and a1 (the acceleration of the first and the last ramps).
    #         a0 = A + B/t0
    #         a1 = A + B/(t - t0)
    # where t is the (new) total duration, t0 is the (new) duration of the first ramp, and
    #         A = (v1 - v0)/t
    #         B = (2d/t) - (v0 + v1).
    newDurInverse = mp.fdiv(one, newDuration)
    A = Mul(Sub(v1, v0), newDurInverse)
    B = Sub(Prod([mp.mpf('2'), d, newDurInverse]), Add(v0, v1))

    interval0 = iv.mpf([zero, newDuration]) # initial interval for t0

    # Now consider the interval(s) computed from a0's constraints
    sum1 = Neg(Add(am, A))
    sum2 = Sub(am, A)
    C = mp.fdiv(B, sum1)
    D = mp.fdiv(B, sum2)

    log.debug("\nA = {0}; \nB = {1}; \nC = {2}; \nD = {3}; \nsum1 = {4}; \nsum2 = {5};".\
              format(mp.nstr(A, n=_prec), mp.nstr(B, n=_prec), mp.nstr(C, n=_prec), mp.nstr(D, n=_prec),
                     mp.nstr(sum1, n=_prec), mp.nstr(sum2, n=_prec)))

    if (sum1 > zero):
        # This implied that the duration is too short
        log.debug("the given duration ({0}) is too short.".format(newDuration))
        return ParabolicCurve()
    if (sum2 < zero):
        # This implied that the duration is too short
        log.debug("the given duration ({0}) is too short.".format(newDuration))
        return ParabolicCurve()
    
    if IsEqual(sum1, zero):
        raise NotImplementedError # not yet considered
    elif sum1 > epsilon:
        log.debug("sum1 > 0. This implies that newDuration is too short.")
        return ParabolicCurve()
    else:
        interval1 = iv.mpf([C, inf])
        
    if IsEqual(sum2, zero):
        raise NotImplementedError # not yet considered
    elif sum2 > epsilon:
        interval2 = iv.mpf([D, inf])
    else:
        log.debug("sum2 < 0. This implies that newDuration is too short.")
        return ParabolicCurve()
        
    if Sub(interval2.a, interval1.b) > epsilon or Sub(interval1.a, interval2.b) > epsilon:
        # interval1 and interval2 do not intersect each other
        return ParabolicCurve()    
    # interval3 = interval1 \cap interval2 : valid interval for t0 computed from a0's constraints
    interval3 = iv.mpf([max(interval1.a, interval2.a), min(interval1.b, interval2.b)])
    
    # Now consider the interval(s) computed from a1's constraints
    if IsEqual(sum1, zero):
        raise NotImplementedError # not yet considered
    elif sum1 > epsilon:
        log.debug("sum1 > 0. This implies that newDuration is too short.")
        return ParabolicCurve()
    else:
        interval4 = iv.mpf([Neg(inf), Add(C, newDuration)])
        
    if IsEqual(sum2, zero):
        raise NotImplementedError # not yet considered
    elif sum2 > epsilon:
        interval5 = iv.mpf([Neg(inf), Add(D, newDuration)])
    else:
        log.debug("sum2 < 0. This implies that newDuration is too short.")
        return ParabolicCurve()

    if Sub(interval5.a, interval4.b) > epsilon or Sub(interval4.a, interval5.b) > epsilon:
        log.debug("interval4 and interval5 do not intersect each other")
        return ParabolicCurve()
    # interval6 = interval4 \cap interval5 : valid interval for t0 computed from a1's constraints
    interval6 = iv.mpf([max(interval4.a, interval5.a), min(interval4.b, interval5.b)])

    # import IPython; IPython.embed()

    if Sub(interval3.a, interval6.b) > epsilon or Sub(interval6.a, interval3.b) > epsilon:
        log.debug("interval3 and interval6 do not intersect each other")
        return ParabolicCurve()
    # interval7 = interval3 \cap interval6
    interval7 = iv.mpf([max(interval3.a, interval6.a), min(interval3.b, interval6.b)])

    if Sub(interval0.a, interval7.b) > epsilon or Sub(interval7.a, interval0.b) > epsilon:
        log.debug("interval0 and interval7 do not intersect each other")
        return ParabolicCurve()
    # interval8 = interval0 \cap interval7 : valid interval of t0 when considering all constraints (from a0 and a1)
    interval8 = iv.mpf([max(interval0.a, interval7.a), min(interval0.b, interval7.b)])

    # import IPython; IPython.embed()
    
    # We choose the value t0 (the duration of the first ramp) by selecting the mid point of the
    # valid interval of t0.
    
    t0 = _SolveForT0(A, B, newDuration, interval8)
    if t0 is None:
        # The fancy procedure fails. Now consider no optimization whatsoever.
        # TODO: Figure out why solving fails.
        t0 = mp.convert(interval8.mid) # select the midpoint
        # return ParabolicCurve()
    t1 = Sub(newDuration, t0)

    a0 = Add(A, Mul(mp.fdiv(one, t0), B))
    if (Abs(t1) < epsilon):
        a1 = zero
    else:
        a1 = Add(A, Mul(mp.fdiv(one, Neg(t1)), B))
    assert(Sub(Abs(a0), am) < epsilon) # check if a0 is really below the bound
    assert(Sub(Abs(a1), am) < epsilon) # check if a1 is really below the bound

    # import IPython; IPython.embed()
    
    # Check if the velocity bound is violated    
    vp = Add(v0, Mul(a0, t0))
    if Abs(vp) > vm:
        vmnew = Mul(mp.sign(vp), vm)
        D2 = Prod([pointfive, Sqr(Sub(vp, vmnew)), Sub(mp.fdiv(one, a0), mp.fdiv(one, a1))])
        # print("D2", end=' ')
        # mp.nprint(D2, n=_prec)
        # print("vmnew", end=' ')
        # mp.nprint(vmnew, n=_prec)
        A2 = Sqr(Sub(vmnew, v0))
        B2 = Neg(Sqr(Sub(vmnew, v1)))
        t0trimmed = mp.fdiv(Sub(vmnew, v0), a0)
        t1trimmed = mp.fdiv(Sub(v1, vmnew), a1)
        C2 = Sum([Mul(t0trimmed, Sub(vmnew, v0)), Mul(t1trimmed, Sub(vmnew, v1)), Mul(mp.mpf('-2'), D2)])

        log.debug("\nA2 = {0}; \nB2 = {1}; \nC2 = {2}; \nD2 = {3};".format(mp.nstr(A2, n=_prec), mp.nstr(B2, n=_prec), mp.nstr(C2, n=_prec), mp.nstr(D2, n=_prec)))
        
        temp = Prod([A2, B2, B2])
        initguess = mp.sign(temp)*(Abs(temp)**(1./3.))
        root = mp.findroot(lambda x: Sub(Prod([x, x, x]), temp), x0=initguess)

        # import IPython; IPython.embed()
        log.debug("root = {0}".format(mp.nstr(root, n=_prec)))
        a0new = mp.fdiv(Add(A2, root), C2)
        if (Abs(a0new) > Add(am, epsilon)):
            if FuzzyZero(Sub(Mul(C2, a0new), A2), epsilon):
                # The computed a0new is exceeding the bound and its corresponding a1new is
                # zero. Therefore, there is no other way to fix this. This is probably because the
                # given newDuration is less than the minimum duration (x0, x1, v0, v1, vm, am) can
                # get.
                log.debug("abs(a0new) > am and a1new = 0; Cannot fix this case. This happens probably because the given newDuration is too short.")
                return ParabolicCurve()
            
            a0new = Mul(mp.sign(a0new), am)

        if (Abs(a0new) < epsilon):
            a1new = mp.fdiv(B2, C2)
            if (Abs(a1new) > Add(am, epsilon)):
                # Similar to the case above
                log.debug("a0new = 0 and abs(a1new) > am; Cannot fix this case. This happens probably because the given newDuration is too short.")
                return ParabolicCurve()

        else:
            if FuzzyZero(Sub(Mul(C2, a0new), A2), epsilon):
                # import IPython; IPython.embed()
                a1new = 0
            else:
                a1new = Mul(mp.fdiv(B2, C2), Add(one, mp.fdiv(A2, Sub(Mul(C2, a0new), A2))))
                if (Abs(a1new) > Add(am, epsilon)):
                    a1new = Mul(mp.sign(a1new), am)
                    a0new = Mul(mp.fdiv(A2, C2), Add(one, mp.fdiv(B2, Sub(Mul(C2, a1new), B2))))

        if (Abs(a0new) > Add(am, epsilon)) or (Abs(a1new) > Add(am, epsilon)):
            log.warn("Cannot fix acceleration bounds violation")
            return ParabolicCurve()        

        log.debug("\na0 = {0}; \na0new = {1}; \na1 = {2}; \na1new = {3};".format(mp.nstr(a0, n=_prec), mp.nstr(a0new, n=_prec), mp.nstr(a1, n=_prec), mp.nstr(a1new, n=_prec)))
        
        if (Abs(a0new) < epsilon) and (Abs(a1new) < epsilon):
            log.warn("Both accelerations are zero. Should we allow this case?")
            return ParabolicCurve()

        if (Abs(a0new) < epsilon):
            # This is likely because v0 is at the velocity bound
            t1new = mp.fdiv(Sub(v1, vmnew), a1new)
            assert(t1new > 0)
            ramp2 = Ramp(v0, a1new, t1new)

            t0new = Sub(newDuration, t1new)
            assert(t0new > 0)
            ramp1 = Ramp(v0, zero, t0new, x0)
            newCurve = ParabolicCurve([ramp1, ramp2])
            return newCurve

        elif (Abs(a1new) < epsilon):
            t0new = mp.fdiv(Sub(vmnew, v0), a0new)
            assert(t0new > 0)
            ramp1 = Ramp(v0, a0new, t0new, x0)
            
            t1new = Sub(newDuration, t0new)
            assert(t1new > 0)
            ramp2 = Ramp(ramp1.v1, zero, t1new)
            newCurve = ParabolicCurve([ramp1, ramp2])
            return newCurve

        else:
            # No problem with those new accelerations
            # import IPython; IPython.embed()
            t0new = mp.fdiv(Sub(vmnew, v0), a0new)
            if (t0new < 0):
                log.debug("t0new < 0. The given newDuration not achievable with the given bounds")
                return ParabolicCurve()
            
            t1new = mp.fdiv(Sub(v1, vmnew), a1new)
            if (t1new < 0):
                log.debug("t1new < 0. The given newDuration not achievable with the given bounds")
                return ParabolicCurve()
            
            if (Add(t0new, t1new) > newDuration):
                # Final fix. Since we give more weight to acceleration bounds, we make the velocity
                # bound saturated. Therefore, we set vp to vmnew.

                # import IPython; IPython.embed()
                if FuzzyZero(A, epsilon):
                    log.warn("(final fix) A is zero. Don't know how to fix this case")
                    return ParabolicCurve()

                t0new = mp.fdiv(Sub(Sub(vmnew, v0), B), A)
                if (t0new < 0):
                    log.debug("(final fix) t0new is negative")
                    return ParabolicCurve()

                t1new = Sub(newDuration, t0new)
                
                a0new = Add(A, Mul(mp.fdiv(one, t0new), B))
                a1new = Add(A, Mul(mp.fdiv(one, Neg(t1new)), B))
                ramp1 = Ramp(v0, a0new, t0new, x0)
                ramp2 = Ramp(ramp1.v1, a1new, t1new)
                newCurve = ParabolicCurve([ramp1, ramp2])

            else:
                ramp1 = Ramp(v0, a0new, t0new, x0)
                ramp3 = Ramp(ramp1.v1, a1new, t1new)
                ramp2 = Ramp(ramp1.v1, zero, Sub(newDuration, Add(t0new , t1new)))
                newCurve = ParabolicCurve([ramp1, ramp2, ramp3])
                
                # import IPython; IPython.embed()
            
            return newCurve
    else:    
        ramp1 = Ramp(v0, a0, t0, x0)
        ramp2 = Ramp(ramp1.v1, a1, t1)
        newCurve = ParabolicCurve([ramp1, ramp2])
        return newCurve
Exemple #13
0
def segregate(n):
    n = int(mp.floor(mp.sqrt(n)) + 1)
    n_d = int(mp.ceil(mp.fdiv(n, pro_cnt)))
    n_list = [[x, x + n_d] for x in range(1, n, n_d)]
    n_list[0][0], n_list[-1][1] = 2, n
    return n_list, len(n_list)
def dipole(r, theta, charge, a):
    '''
    Purpose      : To calculate Electric Potential due to Dipole considering very small values

    Formula Used :

            (q*const_k)*(1/r_1 - 1/r_2)

                   where,
                        r_1     = distance between negative charge and point of observation
                                = (r^2 + a^2 - 2*a*Cos(theta))**0.5
                        r_2     = distance between positive charge and point of observation
                                = (r^2 + a^2 + 2*a*Cos(theta))**0.5
                        const_k = 4*pi*epsilon_not
                                = 8.9875518 x 10^9

    Parameters   :
                   a) r      - distance between center of the dipole and point of observation
                   b) theta  - Angle between positive charge and point of observation
                   c) charge - either charge irrespective of sign
                   d) a      - distance between either charge and center of dipole

    Return: returns exact value of electric field calculated
    '''

    # Editing value of r as per the unit
    if r[1].lower() in ('meters', 'm'):
        r = r[0]
    elif r[1].lower() in ('centimeters', 'cm'):
        r = mp.fmul(r[0], 10**(-2))
    elif r[1].lower() in ('millimeters', 'mm'):
        r = mp.fmul(r[0], 10**(-3))
    elif r[1].lower() in ('angstroms', 'a', 'A'):
        r = mp.fmul(r[0], 10**(-10))

    # Editing value of a as per the unit
    if a[1].lower() in ('meters', 'm'):
        a = a[0]
    elif a[1].lower() in ('centimeters', 'cm'):
        a = mp.fmul(a[0], 10**(-2))
    elif a[1].lower() in ('millimeters', 'mm'):
        a = mp.fmul(a[0], 10**(-3))
    elif a[1].lower() in ('angstroms', 'a', 'A'):
        a = mp.fmul(a[0], 10**(-10))

    # Calculating Value of Cos(theta)
    if theta[1].lower() == 'radians':
        cos_theta = round(mp.cos(theta[0]), 5)
    elif theta[1].lower() == 'degrees':
        cos_theta = round(mp.cos(mp.radians(theta[0])), 5)

    # Editing Value of charge as per the unit
    if charge[1] in ('Coulomb', 'C'):
        charge = charge[0]
    elif charge[1] in ('microCoulomb', 'uC'):
        charge = mp.fmul(charge[0], 10**(-6))
    elif charge[1] in ('milliCoulomb', 'mC'):
        charge = mp.fmul(charge[0], 10**(-3))
    elif charge[1] in ('electronCharge', 'eC'):
        charge = mp.fmul(charge[0], mp.fmul(1.60217646,
                                            mp.fmul(10,
                                                    -19)))  # 1.60217646⋅10-19
    elif charge[1] in ('nanoCoulomb', 'nC'):
        charge = mp.fmul(charge[0], mp.power(10, -10))
    elif charge[1] in ('picoCharge', 'pC'):
        charge = mp.fmul(charge[0], mp.power(10, -12))

    # Calculating value of r_1 and r_2
    r_1 = mp.sqrt(
        mp.fsub(mp.fadd(mp.power(r, 2), mp.power(a, 2)),
                mp.fmul(2, mp.fmul(a, mp.fmul(r, cos_theta)))))
    r_2 = mp.sqrt(
        mp.fadd(mp.fadd(mp.power(r, 2), mp.power(a, 2)),
                mp.fmul(2, mp.fmul(a, mp.fmul(r, cos_theta)))))

    # Calculating final result
    result = mp.fmul(mp.fmul(charge, const_k),
                     mp.fsub(mp.fdiv(1, r_1), mp.fdiv(1, r_2)))

    # returning final result
    return result
Exemple #15
0
def phi3(x0, *, dps):  # (epx(x)-1-x-0.5*x*x)/(x*x*x) , |x| > 1e-32 -> dps > 100
    with mp.workdps(dps):
        y = mp.matrix([mp.fdiv(mp.fsub(mp.fsub(mp.expm1(x), x), mp.fmul('0.5', mp.fmul(x, x))),
                               mp.power(x, '3')) if x != 0.0 else mpf(1)/mpf(6) for x in x0])
        return np.array(y.tolist(), dtype=np.float64)[:, 0]
Exemple #16
0
def _CalculateLeastUpperBoundInoperativeInterval(x0, x1, v0, v1, vm, am):
    # All input must already be of mp.mpf type
    d = x1 - x0
    temp1 = Prod([
        number('2'),
        Neg(Sqr(am)),
        Sub(Prod([number('2'), am, d]), Add(Sqr(v0), Sqr(v1)))
    ])
    if temp1 < zero:
        T0 = number('-1')
        T1 = number('-1')
    else:
        term1 = mp.fdiv(Add(v0, v1), am)
        term2 = mp.fdiv(mp.sqrt(temp1), Sqr(am))
        T0 = Add(term1, term2)
        T1 = Sub(term1, term2)

    temp2 = Prod([
        number('2'),
        Sqr(am),
        Add(Prod([number('2'), am, d]), Add(Sqr(v0), Sqr(v1)))
    ])
    if temp2 < zero:
        T2 = number('-1')
        T3 = number('-1')
    else:
        term1 = Neg(mp.fdiv(Add(v0, v1), am))
        term2 = mp.fdiv(mp.sqrt(temp2), Sqr(am))
        T2 = Add(term1, term2)
        T3 = Sub(term1, term2)

    newDuration = max(max(T0, T1), max(T2, T3))
    if newDuration > zero:
        dStraight = Prod([pointfive, Add(v0, v1), newDuration])
        if Sub(d, dStraight) > 0:
            amNew = am
            vmNew = vm
        else:
            amNew = -am
            vmNew = -vm

        # import IPython; IPython.embed()

        vp = Mul(pointfive, Sum([Mul(newDuration, amNew), v0,
                                 v1]))  # the peak velocity
        if (Abs(vp) > vm):
            dExcess = mp.fdiv(Sqr(Sub(vp, vmNew)), am)
            assert (dExcess > 0)
            deltaTime = mp.fdiv(dExcess, vm)
            newDuration = Add(newDuration, deltaTime)

        log.debug(
            'Calculation successful: T0 = {0}; T1 = {1}; T2 = {2}; T3 = {3}'.
            format(mp.nstr(T0, n=_prec), mp.nstr(T1, n=_prec),
                   mp.nstr(T2, n=_prec), mp.nstr(T3, n=_prec)))

        newDuration = Mul(newDuration, number('1.01'))  # add 1% safety bound
        return newDuration
    else:
        if (FuzzyEquals(x0, x1, epsilon) and FuzzyZero(v0, epsilon)
                and FuzzyZero(v1, epsilon)):
            # t = 0 is actually a correct solution
            newDuration = 0
            return newDuration
        log.debug('Unable to calculate the least upper bound: T0 = {0}; T1 = {1}; T2 = {2}; T3 = {3}'.\
                  format(mp.nstr(T0, n=_prec), mp.nstr(T1, n=_prec), mp.nstr(T2, n=_prec), mp.nstr(T3, n=_prec)))
        return number('-1')
        e.append(strain_rate)
    return e

def simulate_slip_rate(width_min, width_max): #constant strain rate condition
    e = 1.0E-11
    v = []
    for x in range(width_min, width_max):
        slip_rate = (e*31536000)*(x*1000)
        v.append(slip_rate)
    return v

## Simulators, this is for future work/ work in progress.
from mpmath import mp

#define conversion from mm/yr to mm/sec
conv = float(mp.fdiv(1, 31536000))

#grain_size = [5, 6, 7, 8, 9, 10]
def simulate_width(slip_rate, strain_rate):
    #v=w*e
    width = []
    for slip in slip_rate:
        for strain in strain_rate:
            w = (slip/(strain*31536000))/1000
            if w < 30 or w == 30:
                
                width.append(w)
            else:
                width.append(str("ERROR: Too wide"))
            
    return width
Exemple #18
0
def _Stretch1D(curve, newDuration, vm, am):

    log.debug("\nx0 = {0}; x1 = {1}; v0 = {2}; v1 = {3}; vm = {4}; am = {5}; prevDuration = {6}; newDuration = {7}".\
              format(mp.nstr(curve.x0, n=_prec), mp.nstr(curve.EvalPos(curve.duration), n=_prec),
                     mp.nstr(curve.v0, n=_prec), mp.nstr(curve.EvalVel(curve.duration), n=_prec),
                     mp.nstr(vm, n=_prec), mp.nstr(am, n=_prec), mp.nstr(curve.duration, n=_prec),
                     mp.nstr(newDuration, n=_prec)))
    
    # Check types
    if type(newDuration) is not mp.mpf:
        newDuration = mp.mpf("{:.15e}".format(newDuration))
    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(newDuration > curve.duration)
    assert(vm > zero)
    assert(am > zero)

    if (newDuration < -epsilon):
        return ParabolicCurve()
    if (newDuration <= epsilon):
        # Check if this is a stationary trajectory
        if (FuzzyEquals(curve.x0, curve.EvalPos(x1), epsilon) and FuzzyEquals(curve.v0, curve.v1, epsilon)):
            ramp0 = Ramp(curve.v0, 0, 0, curve.x0)
            newCurve = ParabolicCurve(ramp0)
            return newCurve
        else:
            # newDuration is too short to any movement to be made
            return ParabolicCurve()

    v0 = curve[0].v0
    v1 = curve[-1].v1
    d = curve.d

    # First assume no velocity bound -> re-interpolated trajectory will have only two ramps.
    # Solve for a0 and a1 (the acceleration of the first and the last ramps).
    #         a0 = A + B/t0
    #         a1 = A + B/(t - t0)
    # where t is the (new) total duration, t0 is the (new) duration of the first ramp, and
    #         A = (v1 - v0)/t
    #         B = (2d/t) - (v0 + v1).
    newDurInverse = mp.fdiv(one, newDuration)
    A = Mul(Sub(v1, v0), newDurInverse)
    B = Sub(Prod([mp.mpf('2'), d, newDurInverse]), Add(v0, v1))

    interval0 = iv.mpf([zero, newDuration]) # initial interval for t0

    # Now consider the interval(s) computed from a0's constraints
    sum1 = Neg(Add(am, A))
    sum2 = Sub(am, A)
    C = mp.fdiv(B, sum1)
    D = mp.fdiv(B, sum2)

    log.debug("\nA = {0}; \nB = {1}; \nC = {2}; \nD = {3}; \nsum1 = {4}; \nsum2 = {5};".\
              format(mp.nstr(A, n=_prec), mp.nstr(B, n=_prec), mp.nstr(C, n=_prec), mp.nstr(D, n=_prec),
                     mp.nstr(sum1, n=_prec), mp.nstr(sum2, n=_prec)))

    assert(not (sum1 > zero))
    assert(not (sum2 < zero))
    
    if IsEqual(sum1, zero):
        raise NotImplementedError # not yet considered
    elif sum1 > epsilon:
        log.debug("sum1 > 0. This implies that newDuration is too short.")
        return ParabolicCurve()
    else:
        interval1 = iv.mpf([C, inf])
        
    if IsEqual(sum2, zero):
        raise NotImplementedError # not yet considered
    elif sum2 > epsilon:
        interval2 = iv.mpf([D, inf])
    else:
        log.debug("sum2 < 0. This implies that newDuration is too short.")
        return ParabolicCurve()
        
    if Sub(interval2.a, interval1.b) > epsilon or Sub(interval1.a, interval2.b) > epsilon:
        # interval1 and interval2 do not intersect each other
        return ParabolicCurve()    
    # interval3 = interval1 \cap interval2 : valid interval for t0 computed from a0's constraints
    interval3 = iv.mpf([max(interval1.a, interval2.a), min(interval1.b, interval2.b)])
    
    # Now consider the interval(s) computed from a1's constraints
    if IsEqual(sum1, zero):
        raise NotImplementedError # not yet considered
    elif sum1 > epsilon:
        log.debug("sum1 > 0. This implies that newDuration is too short.")
        return ParabolicCurve()
    else:
        interval4 = iv.mpf([Neg(inf), Add(C, newDuration)])
        
    if IsEqual(sum2, zero):
        raise NotImplementedError # not yet considered
    elif sum2 > epsilon:
        interval5 = iv.mpf([Neg(inf), Add(D, newDuration)])
    else:
        log.debug("sum2 < 0. This implies that newDuration is too short.")
        return ParabolicCurve()

    if Sub(interval5.a, interval4.b) > epsilon or Sub(interval4.a, interval5.b) > epsilon:
        # interval4 and interval5 do not intersect each other
        return ParabolicCurve()
    # interval6 = interval4 \cap interval5 : valid interval for t0 computed from a1's constraints
    interval6 = iv.mpf([max(interval4.a, interval5.a), min(interval4.b, interval5.b)])

    # import IPython; IPython.embed()

    if Sub(interval3.a, interval6.b) > epsilon or Sub(interval6.a, interval3.b) > epsilon:
        # interval3 and interval6 do not intersect each other
        return ParabolicCurve()
    # interval7 = interval3 \cap interval6
    interval7 = iv.mpf([max(interval3.a, interval6.a), min(interval3.b, interval6.b)])

    if Sub(interval0.a, interval7.b) > epsilon or Sub(interval7.a, interval0.b) > epsilon:
        # interval0 and interval7 do not intersect each other
        return ParabolicCurve()
    # interval8 = interval0 \cap interval7 : valid interval of t0 when considering all constraints (from a0 and a1)
    interval8 = iv.mpf([max(interval0.a, interval7.a), min(interval0.b, interval7.b)])

    # import IPython; IPython.embed()
    
    # We choose the value t0 (the duration of the first ramp) by selecting the mid point of the
    # valid interval of t0.
    
    t0 = _SolveForT0(A, B, newDuration, interval8)
    if t0 is None:
        # The fancy procedure fails. Now consider no optimization whatsoever.
        # TODO: Figure out why solving fails.
        t0 = mp.convert(interval8.mid) # select the midpoint
        # return ParabolicCurve()
    t1 = Sub(newDuration, t0)

    a0 = Add(A, Mul(mp.fdiv(one, t0), B))
    if (Abs(t1) < epsilon):
        a1 = zero
    else:
        a1 = Add(A, Mul(mp.fdiv(one, Neg(t1)), B))
    assert(Sub(Abs(a0), am) < epsilon) # check if a0 is really below the bound
    assert(Sub(Abs(a1), am) < epsilon) # check if a1 is really below the bound

    # import IPython; IPython.embed()
    
    # Check if the velocity bound is violated    
    vp = Add(v0, Mul(a0, t0))
    if Abs(vp) > vm:
        vmnew = Mul(mp.sign(vp), vm)
        D2 = Prod([pointfive, Sqr(Sub(vp, vmnew)), Sub(mp.fdiv(one, a0), mp.fdiv(one, a1))])
        # print "D2",
        # mp.nprint(D2, n=_prec)
        # print "vmnew",
        # mp.nprint(vmnew, n=_prec)
        A2 = Sqr(Sub(vmnew, v0))
        B2 = Neg(Sqr(Sub(vmnew, v1)))
        t0trimmed = mp.fdiv(Sub(vmnew, v0), a0)
        t1trimmed = mp.fdiv(Sub(v1, vmnew), a1)
        C2 = Sum([Mul(t0trimmed, Sub(vmnew, v0)), Mul(t1trimmed, Sub(vmnew, v1)), Mul(mp.mpf('-2'), D2)])

        log.debug("\nA2 = {0}; \nB2 = {1}; \nC2 = {2}; \nD2 = {3};".format(mp.nstr(A2, n=_prec), mp.nstr(B2, n=_prec), mp.nstr(C2, n=_prec), mp.nstr(D2, n=_prec)))
        
        temp = Prod([A2, B2, B2])
        initguess = mp.sign(temp)*(Abs(temp)**(1./3.))
        root = mp.findroot(lambda x: Sub(Prod([x, x, x]), temp), x0=initguess)

        # import IPython; IPython.embed()
        log.debug("root = {0}".format(mp.nstr(root, n=_prec)))
        a0new = mp.fdiv(Add(A2, root), C2)
        if (Abs(a0new) > Add(am, epsilon)):
            a0new = Mul(mp.sign(a0new), am)

        if (Abs(a0new) < epsilon):
            a1new = mp.fdiv(B2, C2)
            if (Abs(a1new) > Add(am, epsilon)):
                log.warn("abs(a1new) > am; cannot fix this case")
                # Cannot fix this case
                return ParabolicCurve()

        else:
            if FuzzyZero(Sub(Mul(C2, a0new), A2), epsilon):
                # import IPython; IPython.embed()
                a1new = 0
            else:
                a1new = Mul(mp.fdiv(B2, C2), Add(one, mp.fdiv(A2, Sub(Mul(C2, a0new), A2))))
                if (Abs(a1new) > Add(am, epsilon)):
                    a1new = Mul(mp.sign(a1new), am)
                    a0new = Mul(mp.fdiv(A2, C2), Add(one, mp.fdiv(B2, Sub(Mul(C2, a1new), B2))))

        if (Abs(a0new) > Add(am, epsilon)) or (Abs(a1new) > Add(am, epsilon)):
            log.warn("Cannot fix acceleration bounds violation")
            return ParabolicCurve()        

        log.debug("\na0 = {0}; \na0new = {1}; \na1 = {2}; \na1new = {3};".format(mp.nstr(a0, n=_prec), mp.nstr(a0new, n=_prec), mp.nstr(a1, n=_prec), mp.nstr(a1new, n=_prec)))
        
        if (Abs(a0new) < epsilon) and (Abs(a1new) < epsilon):
            log.warn("Both accelerations are zero. Should we allow this case?")
            return ParabolicCurve()

        if (Abs(a0new) < epsilon):
            # This is likely because v0 is at the velocity bound
            t1new = mp.fdiv(Sub(v1, vmnew), a1new)
            assert(t1new > 0)
            ramp2 = Ramp(v0, a1new, t1new)

            t0new = Sub(newDuration, t1new)
            assert(t0new > 0)
            ramp1 = Ramp(v0, zero, t0new, curve.x0)
            newCurve = ParabolicCurve([ramp1, ramp2])
            return newCurve

        elif (Abs(a1new) < epsilon):
            t0new = mp.fdiv(Sub(vmnew, v0), a0new)
            assert(t0new > 0)
            ramp1 = Ramp(v0, a0new, t0new, curve.x0)
            
            t1new = Sub(newDuration, t0new)
            assert(t1new > 0)
            ramp2 = Ramp(ramp1.v1, zero, t1new)
            newCurve = ParabolicCurve([ramp1, ramp2])
            return newCurve

        else:
            # No problem with those new accelerations
            # import IPython; IPython.embed()
            t0new = mp.fdiv(Sub(vmnew, v0), a0new)
            assert(t0new > 0)
            t1new = mp.fdiv(Sub(v1, vmnew), a1new)
            assert(t1new > 0)
            if (Add(t0new, t1new) > newDuration):
                # Final fix. Since we give more weight to acceleration bounds, we make the velocity
                # bound saturated. Therefore, we set vp to vmnew.

                # import IPython; IPython.embed()
                t0new = mp.fdiv(Sub(Sub(vmnew, v0), B), A)
                t1new = Sub(newDuration, t0new)
                assert(t1new > zero)
                a0new = Add(A, Mul(mp.fdiv(one, t0new), B))
                a1new = Add(A, Mul(mp.fdiv(one, Neg(t1new)), B))
                ramp1 = Ramp(v0, a0new, t0new, curve.x0)
                ramp2 = Ramp(ramp1.v1, a1new, t1new)
                newCurve = ParabolicCurve([ramp1, ramp2])

            else:
                # t0new = mp.fdiv(Sub(vmnew, v0), a0new)
                # assert(t0new > 0)
                ramp1 = Ramp(v0, a0new, t0new, curve.x0)
                
                # t1new = mp.fdiv(Sub(v1, vmnew), a1new)
                # assert(t1new > 0)
                ramp3 = Ramp(ramp1.v1, a1new, t1new)
                # print "a1",
                # mp.nprint(a1, n=_prec)
                # print "a1new",
                # mp.nprint(a1new, n=_prec)
                
                # print "t0trimmed",
                # mp.nprint(t0trimmed, n=_prec)
                # print "t0new",
                # mp.nprint(t0new, n=_prec)
                # print "t1trimmed", 
                # mp.nprint(t1trimmed, n=_prec)
                # print "t1new", 
                # mp.nprint(t1new, n=_prec)
                # print "T", 
                # mp.nprint(newDuration, n=_prec)
                
                ramp2 = Ramp(ramp1.v1, zero, Sub(newDuration, Add(t0new , t1new)))
                newCurve = ParabolicCurve([ramp1, ramp2, ramp3])
                
                # import IPython; IPython.embed()
            
            return newCurve
    else:    
        ramp1 = Ramp(v0, a0, t0, curve.x0)
        ramp2 = Ramp(ramp1.v1, a1, t1)
        newCurve = ParabolicCurve([ramp1, ramp2])
        return newCurve
Exemple #19
0
        oldp = 1
        p = 0
        oldq = 0
        q = 1
        seq = self.get_seq(n)
        for a in seq[1:]:
            if a == 0:
                break

            temp = p
            p = a*p+oldp
            oldp = temp

            temp = q
            q = a*q+oldq
            oldq = temp

        return seq[0] + Rational(p, q)


if __name__ == '__main__':
    mp.dps = 200
    test_cr = CR(3.75)
    print(test_cr.get_seq(10))
    print(test_cr.get_approximation(10))
    pi_cr = CR(mp.pi)
    print(pi_cr.get_seq(100))
    pi_approx = pi_cr.get_approximation(32)
    print(pi_approx)
    print(mp.fdiv(pi_approx.p, pi_approx.q))
    print(mp.pi)
Exemple #20
0
def Interpolate1DFixedDuration(x0, x1, v0, v1, newDuration, vm, am):
    x0 = ConvertFloatToMPF(x0)
    x1 = ConvertFloatToMPF(x1)
    v0 = ConvertFloatToMPF(v0)
    v1 = ConvertFloatToMPF(v1)
    vm = ConvertFloatToMPF(vm)
    am = ConvertFloatToMPF(am)
    newDuration = ConvertFloatToMPF(newDuration)
    log.debug("\nx0 = {0}; x1 = {1}; v0 = {2}; v1 = {3}; vm = {4}; am = {5}; newDuration = {6}".\
              format(mp.nstr(x0, n=_prec), mp.nstr(x1, n=_prec), mp.nstr(v0, n=_prec), mp.nstr(v1, n=_prec),
                     mp.nstr(vm, n=_prec), mp.nstr(am, n=_prec), mp.nstr(newDuration, n=_prec)))

    # Check inputs
    assert (vm > zero)
    assert (am > zero)

    if (newDuration < -epsilon):
        log.info("duration = {0} is negative".format(newDuration))
        return ParabolicCurve()
    if (newDuration <= epsilon):
        # Check if this is a stationary trajectory
        if (FuzzyEquals(x0, x1, epsilon) and FuzzyEquals(v0, v1, epsilon)):
            log.info("stationary trajectory")
            ramp0 = Ramp(v0, 0, 0, x0)
            newCurve = ParabolicCurve(ramp0)
            return newCurve
        else:
            log.info("newDuration is too short for any movement to be made")
            return ParabolicCurve()

    # Correct small discrepancies if any
    if (v0 > vm):
        if FuzzyEquals(v0, vm, epsilon):
            v0 = vm
        else:
            log.info("v0 > vm: {0} > {1}".format(v0, vm))
            return ParabolicCurve()
    elif (v0 < -vm):
        if FuzzyEquals(v0, -vm, epsilon):
            v0 = -vm
        else:
            log.info("v0 < -vm: {0} < {1}".format(v0, -vm))
            return ParabolicCurve()
    if (v1 > vm):
        if FuzzyEquals(v1, vm, epsilon):
            v1 = vm
        else:
            log.info("v1 > vm: {0} > {1}".format(v1, vm))
            return ParabolicCurve()
    elif (v1 < -vm):
        if FuzzyEquals(v1, -vm, epsilon):
            v1 = -vm
        else:
            log.info("v1 < -vm: {0} < {1}".format(v1, -vm))
            return ParabolicCurve()

    d = Sub(x1, x0)

    # First assume no velocity bound -> re-interpolated trajectory will have only two ramps.
    # Solve for a0 and a1 (the acceleration of the first and the last ramps).
    #         a0 = A + B/t0
    #         a1 = A + B/(t - t0)
    # where t is the (new) total duration, t0 is the (new) duration of the first ramp, and
    #         A = (v1 - v0)/t
    #         B = (2d/t) - (v0 + v1).
    newDurInverse = mp.fdiv(one, newDuration)
    A = Mul(Sub(v1, v0), newDurInverse)
    B = Sub(Prod([mp.mpf('2'), d, newDurInverse]), Add(v0, v1))

    interval0 = iv.mpf([zero, newDuration])  # initial interval for t0

    # Now consider the interval(s) computed from a0's constraints
    sum1 = Neg(Add(am, A))
    sum2 = Sub(am, A)
    C = mp.fdiv(B, sum1)
    D = mp.fdiv(B, sum2)

    log.debug("\nA = {0}; \nB = {1}; \nC = {2}; \nD = {3}; \nsum1 = {4}; \nsum2 = {5};".\
              format(mp.nstr(A, n=_prec), mp.nstr(B, n=_prec), mp.nstr(C, n=_prec), mp.nstr(D, n=_prec),
                     mp.nstr(sum1, n=_prec), mp.nstr(sum2, n=_prec)))

    if (sum1 > zero):
        # This implied that the duration is too short
        log.debug("the given duration ({0}) is too short.".format(newDuration))
        return ParabolicCurve()
    if (sum2 < zero):
        # This implied that the duration is too short
        log.debug("the given duration ({0}) is too short.".format(newDuration))
        return ParabolicCurve()

    if IsEqual(sum1, zero):
        raise NotImplementedError  # not yet considered
    elif sum1 > epsilon:
        log.debug("sum1 > 0. This implies that newDuration is too short.")
        return ParabolicCurve()
    else:
        interval1 = iv.mpf([C, inf])

    if IsEqual(sum2, zero):
        raise NotImplementedError  # not yet considered
    elif sum2 > epsilon:
        interval2 = iv.mpf([D, inf])
    else:
        log.debug("sum2 < 0. This implies that newDuration is too short.")
        return ParabolicCurve()

    if Sub(interval2.a, interval1.b) > epsilon or Sub(interval1.a,
                                                      interval2.b) > epsilon:
        # interval1 and interval2 do not intersect each other
        return ParabolicCurve()
    # interval3 = interval1 \cap interval2 : valid interval for t0 computed from a0's constraints
    interval3 = iv.mpf(
        [max(interval1.a, interval2.a),
         min(interval1.b, interval2.b)])

    # Now consider the interval(s) computed from a1's constraints
    if IsEqual(sum1, zero):
        raise NotImplementedError  # not yet considered
    elif sum1 > epsilon:
        log.debug("sum1 > 0. This implies that newDuration is too short.")
        return ParabolicCurve()
    else:
        interval4 = iv.mpf([Neg(inf), Add(C, newDuration)])

    if IsEqual(sum2, zero):
        raise NotImplementedError  # not yet considered
    elif sum2 > epsilon:
        interval5 = iv.mpf([Neg(inf), Add(D, newDuration)])
    else:
        log.debug("sum2 < 0. This implies that newDuration is too short.")
        return ParabolicCurve()

    if Sub(interval5.a, interval4.b) > epsilon or Sub(interval4.a,
                                                      interval5.b) > epsilon:
        log.debug("interval4 and interval5 do not intersect each other")
        return ParabolicCurve()
    # interval6 = interval4 \cap interval5 : valid interval for t0 computed from a1's constraints
    interval6 = iv.mpf(
        [max(interval4.a, interval5.a),
         min(interval4.b, interval5.b)])

    # import IPython; IPython.embed()

    if Sub(interval3.a, interval6.b) > epsilon or Sub(interval6.a,
                                                      interval3.b) > epsilon:
        log.debug("interval3 and interval6 do not intersect each other")
        return ParabolicCurve()
    # interval7 = interval3 \cap interval6
    interval7 = iv.mpf(
        [max(interval3.a, interval6.a),
         min(interval3.b, interval6.b)])

    if Sub(interval0.a, interval7.b) > epsilon or Sub(interval7.a,
                                                      interval0.b) > epsilon:
        log.debug("interval0 and interval7 do not intersect each other")
        return ParabolicCurve()
    # interval8 = interval0 \cap interval7 : valid interval of t0 when considering all constraints (from a0 and a1)
    interval8 = iv.mpf(
        [max(interval0.a, interval7.a),
         min(interval0.b, interval7.b)])

    # import IPython; IPython.embed()

    # We choose the value t0 (the duration of the first ramp) by selecting the mid point of the
    # valid interval of t0.

    t0 = _SolveForT0(A, B, newDuration, interval8)
    if t0 is None:
        # The fancy procedure fails. Now consider no optimization whatsoever.
        # TODO: Figure out why solving fails.
        t0 = mp.convert(interval8.mid)  # select the midpoint
        # return ParabolicCurve()
    t1 = Sub(newDuration, t0)

    a0 = Add(A, Mul(mp.fdiv(one, t0), B))
    if (Abs(t1) < epsilon):
        a1 = zero
    else:
        a1 = Add(A, Mul(mp.fdiv(one, Neg(t1)), B))
    assert (Sub(Abs(a0), am) < epsilon
            )  # check if a0 is really below the bound
    assert (Sub(Abs(a1), am) < epsilon
            )  # check if a1 is really below the bound

    # import IPython; IPython.embed()

    # Check if the velocity bound is violated
    vp = Add(v0, Mul(a0, t0))
    if Abs(vp) > vm:
        vmnew = Mul(mp.sign(vp), vm)
        D2 = Prod([
            pointfive,
            Sqr(Sub(vp, vmnew)),
            Sub(mp.fdiv(one, a0), mp.fdiv(one, a1))
        ])
        # print("D2", end=' ')
        # mp.nprint(D2, n=_prec)
        # print("vmnew", end=' ')
        # mp.nprint(vmnew, n=_prec)
        A2 = Sqr(Sub(vmnew, v0))
        B2 = Neg(Sqr(Sub(vmnew, v1)))
        t0trimmed = mp.fdiv(Sub(vmnew, v0), a0)
        t1trimmed = mp.fdiv(Sub(v1, vmnew), a1)
        C2 = Sum([
            Mul(t0trimmed, Sub(vmnew, v0)),
            Mul(t1trimmed, Sub(vmnew, v1)),
            Mul(mp.mpf('-2'), D2)
        ])

        log.debug("\nA2 = {0}; \nB2 = {1}; \nC2 = {2}; \nD2 = {3};".format(
            mp.nstr(A2, n=_prec), mp.nstr(B2, n=_prec), mp.nstr(C2, n=_prec),
            mp.nstr(D2, n=_prec)))

        temp = Prod([A2, B2, B2])
        initguess = mp.sign(temp) * (Abs(temp)**(1. / 3.))
        root = mp.findroot(lambda x: Sub(Prod([x, x, x]), temp), x0=initguess)

        # import IPython; IPython.embed()
        log.debug("root = {0}".format(mp.nstr(root, n=_prec)))
        a0new = mp.fdiv(Add(A2, root), C2)
        if (Abs(a0new) > Add(am, epsilon)):
            if FuzzyZero(Sub(Mul(C2, a0new), A2), epsilon):
                # The computed a0new is exceeding the bound and its corresponding a1new is
                # zero. Therefore, there is no other way to fix this. This is probably because the
                # given newDuration is less than the minimum duration (x0, x1, v0, v1, vm, am) can
                # get.
                log.debug(
                    "abs(a0new) > am and a1new = 0; Cannot fix this case. This happens probably because the given newDuration is too short."
                )
                return ParabolicCurve()

            a0new = Mul(mp.sign(a0new), am)

        if (Abs(a0new) < epsilon):
            a1new = mp.fdiv(B2, C2)
            if (Abs(a1new) > Add(am, epsilon)):
                # Similar to the case above
                log.debug(
                    "a0new = 0 and abs(a1new) > am; Cannot fix this case. This happens probably because the given newDuration is too short."
                )
                return ParabolicCurve()

        else:
            if FuzzyZero(Sub(Mul(C2, a0new), A2), epsilon):
                # import IPython; IPython.embed()
                a1new = 0
            else:
                a1new = Mul(mp.fdiv(B2, C2),
                            Add(one, mp.fdiv(A2, Sub(Mul(C2, a0new), A2))))
                if (Abs(a1new) > Add(am, epsilon)):
                    a1new = Mul(mp.sign(a1new), am)
                    a0new = Mul(mp.fdiv(A2, C2),
                                Add(one, mp.fdiv(B2, Sub(Mul(C2, a1new), B2))))

        if (Abs(a0new) > Add(am, epsilon)) or (Abs(a1new) > Add(am, epsilon)):
            log.warn("Cannot fix acceleration bounds violation")
            return ParabolicCurve()

        log.debug(
            "\na0 = {0}; \na0new = {1}; \na1 = {2}; \na1new = {3};".format(
                mp.nstr(a0, n=_prec), mp.nstr(a0new, n=_prec),
                mp.nstr(a1, n=_prec), mp.nstr(a1new, n=_prec)))

        if (Abs(a0new) < epsilon) and (Abs(a1new) < epsilon):
            log.warn("Both accelerations are zero. Should we allow this case?")
            return ParabolicCurve()

        if (Abs(a0new) < epsilon):
            # This is likely because v0 is at the velocity bound
            t1new = mp.fdiv(Sub(v1, vmnew), a1new)
            assert (t1new > 0)
            ramp2 = Ramp(v0, a1new, t1new)

            t0new = Sub(newDuration, t1new)
            assert (t0new > 0)
            ramp1 = Ramp(v0, zero, t0new, x0)
            newCurve = ParabolicCurve([ramp1, ramp2])
            return newCurve

        elif (Abs(a1new) < epsilon):
            t0new = mp.fdiv(Sub(vmnew, v0), a0new)
            assert (t0new > 0)
            ramp1 = Ramp(v0, a0new, t0new, x0)

            t1new = Sub(newDuration, t0new)
            assert (t1new > 0)
            ramp2 = Ramp(ramp1.v1, zero, t1new)
            newCurve = ParabolicCurve([ramp1, ramp2])
            return newCurve

        else:
            # No problem with those new accelerations
            # import IPython; IPython.embed()
            t0new = mp.fdiv(Sub(vmnew, v0), a0new)
            if (t0new < 0):
                log.debug(
                    "t0new < 0. The given newDuration not achievable with the given bounds"
                )
                return ParabolicCurve()

            t1new = mp.fdiv(Sub(v1, vmnew), a1new)
            if (t1new < 0):
                log.debug(
                    "t1new < 0. The given newDuration not achievable with the given bounds"
                )
                return ParabolicCurve()

            if (Add(t0new, t1new) > newDuration):
                # Final fix. Since we give more weight to acceleration bounds, we make the velocity
                # bound saturated. Therefore, we set vp to vmnew.

                # import IPython; IPython.embed()
                if FuzzyZero(A, epsilon):
                    log.warn(
                        "(final fix) A is zero. Don't know how to fix this case"
                    )
                    return ParabolicCurve()

                t0new = mp.fdiv(Sub(Sub(vmnew, v0), B), A)
                if (t0new < 0):
                    log.debug("(final fix) t0new is negative")
                    return ParabolicCurve()

                t1new = Sub(newDuration, t0new)

                a0new = Add(A, Mul(mp.fdiv(one, t0new), B))
                a1new = Add(A, Mul(mp.fdiv(one, Neg(t1new)), B))
                ramp1 = Ramp(v0, a0new, t0new, x0)
                ramp2 = Ramp(ramp1.v1, a1new, t1new)
                newCurve = ParabolicCurve([ramp1, ramp2])

            else:
                ramp1 = Ramp(v0, a0new, t0new, x0)
                ramp3 = Ramp(ramp1.v1, a1new, t1new)
                ramp2 = Ramp(ramp1.v1, zero, Sub(newDuration,
                                                 Add(t0new, t1new)))
                newCurve = ParabolicCurve([ramp1, ramp2, ramp3])

                # import IPython; IPython.embed()

            return newCurve
    else:
        ramp1 = Ramp(v0, a0, t0, x0)
        ramp2 = Ramp(ramp1.v1, a1, t1)
        newCurve = ParabolicCurve([ramp1, ramp2])
        return newCurve
Exemple #21
0
def phi1(x0, *, dps):  # (exp(x) - 1)/x, |x| > 1e-32 -> dps > 16
    with mp.workdps(dps):
        y = mp.matrix(
            [mp.fdiv(mp.expm1(x), x) if x != 0.0 else mpf('1') for x in x0])
        return np.array(y.tolist(), dtype=x0.dtype)[:, 0]
Exemple #22
0
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]
Exemple #23
0
def phi2(x0, *, dps):  # (exp(x) - 1 - x) / (x * x), |x| > 1e-32 -> dps > 40
    with mp.workdps(dps):
        y = mp.matrix([mp.fdiv(mp.fsub(mp.expm1(x), x), mp.fmul(
            x, x)) if x != 0.0 else mpf(1)/mpf(2) for x in x0])
        return np.array(y.tolist(), dtype=np.float64)[:, 0]