コード例 #1
0
ファイル: p011.py プロジェクト: jorgemira/euler-py
def solve():
    """Main function"""
    grid = []
    res = 0
    max_adj = 4
    text = slurp_file("p011.txt")

    for row in text.split("\n"):
        grid.append([int(x) for x in row.split(" ")])

    len_grid = len(grid)

    # Horizontal
    for i in xrange(len_grid):
        for j in xrange(len_grid - max_adj):
            res = max(res, mul([grid[i][j + x] for x in xrange(max_adj)]))

    # Vertical
    for i in xrange(len_grid):
        for j in xrange(len_grid - max_adj):
            res = max(res, mul([grid[j + x][i] for x in xrange(max_adj)]))

    # Diag 1
    for i in xrange(len_grid - max_adj):
        for j in xrange(len_grid - max_adj):
            res = max(res, mul([grid[i + x][j + x] for x in xrange(max_adj)]))

    # Diag 2
    for i in xrange(max_adj - 1, len_grid - max_adj):
        for j in xrange(len_grid - max_adj):
            res = max(res, mul([grid[i - x][j + x] for x in xrange(max_adj)]))

    return res
コード例 #2
0
def gradient_it(m, eps):
    cnt = 0

    (A, B) = ut.split_at(m)
    x0 = [0] * len(B)
    r0 = ut.minus(B, ut.subst(A, x0))
    p0 = r0
    z0 = r0
    s0 = r0

    for i in range(0, 2 * len(B)):
        sub = ut.subst(A, z0)
        prev_p = p0
        prev_r = r0
        At = ut.transpose_matrix(A)
        # print(At)

        a = ut.scalar_product(p0, r0) / ut.scalar_product(s0, sub)
        x0 = ut.plus(x0, ut.mul(a, z0))
        r0 = ut.minus(r0, ut.mul(a, sub))
        p0 = ut.minus(p0, ut.mul(a, ut.subst(At, s0)))
        b = ut.scalar_product(p0, r0) / ut.scalar_product(prev_p, prev_r)
        z0 = ut.plus(r0, ut.mul(b, z0))
        s0 = ut.plus(p0, ut.mul(b, s0))

        if abs(ut.norm(r0) / ut.norm(B)) < eps:
            break
        cnt += 1

    return (x0, cnt)
コード例 #3
0
    def side(self, v0, v1, v2, origin, direction):
        v0v1 = sub(v1, v0)
        v0v2 = sub(v2, v0)

        N = cross(v0v1, v0v2)
        
        raydirection = dot(N, direction)

        if abs(raydirection) < 0.0001:
            return None
        
        d = dot(N, v0)
        
        t = (dot(N, origin) + d) / raydirection
        
        if t < 0:
            return None

        P = sum(origin, mul(direction, t))
        U, V, W = barycentric(v0, v1, v2, P)
        
        if U < 0 or V < 0 or W < 0:
            return None
        else: 
            return Intersect(distance = d,
                         point = P,
                         normal = norm(N))
コード例 #4
0
    def castRay(self, origin, direction):
        material, intersect = self.sceneIntersect(origin, direction)
        
        if material is None:
            return self.currentColor

        lightDir = norm(sub(self.light.position, intersect.point))
        lightDistance = length(sub(self.light.position, intersect.point))
        
        offsetNormal = mul(intersect.normal, 1.1)
        shadowOrigin = sub(intersect.point, offsetNormal) if dot(lightDir, intersect.normal) < 0 else sum(intersect.point, offsetNormal)
        shadowMaterial, shadowIntersect = self.sceneIntersect(shadowOrigin, lightDir)
        shadowIntensity = 0

        if shadowMaterial and length(sub(shadowIntersect.point, shadowOrigin)) < lightDistance:
            shadowIntensity = 0.9

        intensity = self.light.intensity * max(0, dot(lightDir, intersect.normal)) * (1 - shadowIntensity)

        reflection = reflect(lightDir, intersect.normal)
        specularIntensity = self.light.intensity * (
            max(0, -dot(reflection, direction)) ** material.spec
        )

        diffuse = material.diffuse * intensity * material.albedo[0]
        specular = Color(255, 255, 255) * specularIntensity * material.albedo[1]
        return diffuse + specular
コード例 #5
0
    def castRay(self, origin, direction, recursion=0):
        material, intersect = self.sceneIntersect(origin, direction)

        if material is None or recursion >= MAX_RECURSION_DEPTH:
            return self.currentColor
            # Si el rayo no golpeo nada o si llego al limite de recursion

        lightDir = norm(sub(self.light.position, intersect.point))
        lightDistance = length(sub(self.light.position, intersect.point))

        offsetNormal = mul(intersect.normal, 1.1)
        shadowOrigin = sub(
            intersect.point,
            offsetNormal) if dot(lightDir, intersect.normal) < 0 else sum(
                intersect.point, offsetNormal)
        shadowMaterial, shadowIntersect = self.sceneIntersect(
            shadowOrigin, lightDir)
        shadowIntensity = 0

        if shadowMaterial and length(sub(shadowIntersect.point,
                                         shadowOrigin)) < lightDistance:
            shadowIntensity = 0.9

        intensity = self.light.intensity * max(
            0, dot(lightDir, intersect.normal)) * (1 - shadowIntensity)

        reflection = reflect(lightDir, intersect.normal)
        specularIntensity = self.light.intensity * (max(
            0, -dot(reflection, direction))**material.spec)

        if material.albedo[2] > 0:
            reflectDir = reflect(direction, intersect.normal)
            reflectOrigin = sub(intersect.point, offsetNormal) if dot(
                reflectDir, intersect.normal) < 0 else sum(
                    intersect.point, offsetNormal)
            reflectedColor = self.castRay(reflectOrigin, reflectDir,
                                          recursion + 1)
        else:
            reflectedColor = self.currentColor

        if material.albedo[3] > 0:
            refractDir = refract(direction, intersect.normal,
                                 material.refractionIndex)
            refractOrigin = sub(intersect.point, offsetNormal) if dot(
                refractDir, intersect.normal) < 0 else sum(
                    intersect.point, offsetNormal)
            refractedColor = self.castRay(refractOrigin, refractDir,
                                          recursion + 1)
        else:
            refractedColor = self.currentColor

        diffuse = material.diffuse * intensity * material.albedo[0]
        specular = Color(255, 255,
                         255) * specularIntensity * material.albedo[1]
        reflected = reflectedColor * material.albedo[2]
        refracted = refractedColor * material.albedo[3]

        return diffuse + specular + reflected + refracted
コード例 #6
0
def gauss(m):
    for i in range(len(m) - 1):
        # print(m)
        max_el = 0
        ind = i

        for j in range(i, len(m[0]) - 1):
            # print(abs(m[j][i]))
            if abs(m[j][i]) > max_el:
                max_el = abs(m[j][i])
                ind = j

        # print(max_el)
        swap_str(m, ind, i)
        for j in range(i + 1, len(m)):
            # c = m[j][i] / m[i][i]

            if m[i][i] == 0:
                continue

            # print(m[j][i])
            # print(m[i][i])
            # print(m[j][i] / m[i][i])
            mul_str = mul(m[j][i] / m[i][i], m[i])
            m[j] = minus(m[j], mul_str)
            for k in range(len(m[j]) - 1):
                if m[j][k] < 0.000001:
                    m[j][k] = 0

    # print(m)
    x = [0] * len(m)
    for i in range(len(m) - 1, -1, -1):
        a = 0

        for j in range(i + 1, (len(m))):
            a = a + m[i][j] * x[j]

        b = m[i][len(m[0]) - 1] - a

        if m[i][i] != 0:
            x[i] = b / m[i][i]
        else:
            x[i] = 0
            # if b == 0:
            #     x[i] = 0
            # else:
            #     return []

    return x


# print(gauss([[2, 5, 1, 4], [3, 1, -2, -5], [1, 1, 1, 6]]))
# print(gauss([[-1, -1, 3], [-0.3333, -1, 2.3333]]))
# print(gauss([[-1, 0.5, -1.5, -1, -0.5], [-3, -1, -2, -3, -1], [-0.25, -0.75, -1, -0.25, -0.25], [-0.25, -0.5, -0.25, -1, -0.25]]))
# print(gauss([[-1, -0.666666, -0.5, -0.4, -0.333334, -6], [-1.33333, -1, -0.8, -0.666668, -0.571428, -16], [-1.5, -1.2, -1, -0.85714, -0.749999, -59.9999], [-1.6, -1.33334, -1.14286, -1, -0.888888, -24], [-1.66667, -1.42857, -1.25, -1.11111, -1, -550]]))
コード例 #7
0
def chinese_remainder_theorem(discs):
    M = mul(d[0] for d in discs)
    x = 0

    for i, (size, initial) in enumerate(discs, start=1):
        # Disc #2 has 17 positions; at time=0, it is at position 15.
        # => x \equiv (17 - 15 - 2) (mod 17)
        M_i = (M / size)
        x += (size - initial - i) * M_i * modinv(M_i, size)

    return x
コード例 #8
0
ファイル: p024.py プロジェクト: jorgemira/euler-py
def solve():
    '''Main function'''
    top = 10
    nth = 1000000

    nth -= 1
    nums = range(top)
    res = ""

    for i in reversed(xrange(top)):
        tmp = mul(xrange(1, i+1))
        res += str(nums.pop(nth/tmp))
        nth %= tmp

    return int(res)
コード例 #9
0
    def rayIntersect(self, orig, dir):
        # t = (( position - origRayo) dot normal) / (dirRayo dot normal)

        denom = dot(dir, self.normal)

        if abs(denom) > 0.0001:
            t = dot(self.normal, sub(self.position, orig)) / denom
            if t > 0:
                # P = O + tD
                hit = sum(orig, mul(dir, t))

                return Intersect(distance = t,
                                 point = hit,
                                 normal = self.normal)

        return None
コード例 #10
0
ファイル: sphere.py プロジェクト: PabloLopez10/Graficas
 def rayIntersect(self, origin, direction):
   L = sub(self.center, origin)
   tca = dot(L, direction)
   l = length(L)
   d2 = l ** 2 - tca ** 2
   if d2 > self.radius ** 2:
       return None
   thc = (self.radius ** 2 - d2) ** 0.5
   t0 = tca - thc
   t1 = tca + thc
   if t0 < 0:
       t0 = t1
   if t0 < 0:
       return None
   hit = sum(origin, mul(direction, t0))
   normal = norm(sub(hit, self.center))
   return Intersect(
       distance=t0,
       point=hit,
       normal=normal
   )
コード例 #11
0
ファイル: day09.py プロジェクト: iKevinY/advent
                    break
        else:
            LOW_POINTS.add(p)
            part_1 += (val + 1)

print "Part 1:", part_1


# Solve part 2.
def compute_basin_size(board, start):
    """Return the set of all explored points from a given point."""
    seen = set()

    def _dfs(node):
        val = board[node]
        seen.add(node)
        for n in node.neighbours_4():
            if n in board and n not in seen:
                if val <= board[n] and board[n] != 9:
                    _dfs(n)

    _dfs(start)
    return len(seen)

BASIN_SIZES = {}
for p in LOW_POINTS:
    BASIN_SIZES[p] = compute_basin_size(BOARD, p)

print "Part 2:", mul(sorted(BASIN_SIZES.values(), reverse=True)[:3])

コード例 #12
0
 def contr(self, x, t, x_ref=0, u_ref=0):
     contr = mul(self.Ks[t], x - x_ref) + self.ks[t] + u_ref
     return contr
コード例 #13
0
    def backups(self, env, T, Cs, Fs, cs, fs):
        print "Computing backwards pass"
        start_time = timer.time()

        n = env.state_dim
        d = env.action_dim

        C1, C2, C3, C4 = utils.breakup_C(Cs[-1], n)
        c1, c2 = utils.breakup_c(cs[-1], n)

        K_T = -np.dot(np.linalg.inv(C4), C3)
        k_T = -np.dot(np.linalg.inv(C4), c2)

        V_T = C1 + mul(C2, K_T) + mul(K_T.T, C3) + mul(K_T.T, C4, K_T)
        v_T = c1 + mul(C2, k_T) + mul(K_T.T, c2) + mul(K_T.T, C4, k_T)

        Vs, vs, Ks, ks = [None] * T, [None] * T, [None] * T, [None] * T
        Vs[-1], vs[-1], Ks[-1], ks[-1] = V_T, v_T, K_T, k_T

        for t in range(T - 1)[::-1]:
            C_t = Cs[t]
            F_t = Fs[t]
            c_t = cs[t]
            f_t = fs[t]

            # IPython.embed()
            Q_t = C_t + mul(F_t.T, Vs[t + 1], F_t)
            q_t = c_t + mul(F_t.T, Vs[t + 1], f_t) + mul(F_t.T, vs[t + 1])

            Q1, Q2, Q3, Q4 = utils.breakup_C(Q_t, n)
            q1, q2 = utils.breakup_c(q_t, n)
            Ks[t] = -mul(inv(Q4), Q3)
            ks[t] = -mul(inv(Q4), q2)
            Vs[t] = Q1 + mul(Q2, Ks[t]) + mul(Ks[t].T, Q3) + mul(
                Ks[t].T, Q4, Ks[t])
            vs[t] = q1 + mul(Q2, ks[t]) + mul(Ks[t].T, q2) + mul(
                Ks[t].T, Q4, ks[t])

        self.Vs = Vs
        self.vs = vs
        self.Ks = Ks
        self.ks = ks
        print "Done computing back pass"
        end_time = timer.time()
        print "Total time: " + str(end_time - start_time)
        return Vs, vs, Ks, ks
コード例 #14
0
    def __init__(self,
                 numberOfInducingPoints,  # Number of inducing ponts in sparse GP
                 batchSize,              # Size of mini batch
                 dimX,                   # Dimensionality of the latent co-ordinates
                 dimZ,                   # Dimensionality of the latent variables
                 data,                   # [NxP] matrix of observations
                 kernelType='ARD',
                 encoderType_qX='FreeForm2',  # 'MLP', 'Kernel'.
                 encoderType_rX='FreeForm2',  # 'MLP', 'Kernel'
                 Xu_optimise=False,
                 numberOfEncoderHiddenUnits=10
                 ):

        self.numTestSamples = 5000

        # set the data
        data = np.asarray(data, dtype=precision)
        self.N = data.shape[0]  # Number of observations
        self.P = data.shape[1]  # Dimension of each observation
        self.M = numberOfInducingPoints
        self.B = batchSize
        self.R = dimX
        self.Q = dimZ
        self.H = numberOfEncoderHiddenUnits

        self.encoderType_qX = encoderType_qX
        self.encoderType_rX = encoderType_rX
        self.Xu_optimise = Xu_optimise

        self.y = th.shared(data)
        self.y.name = 'y'

        if kernelType == 'RBF':
            self.numberOfKernelParameters = 2
        elif kernelType == 'RBFnn':
            self.numberOfKernelParameters = 1
        elif kernelType == 'ARD':
            self.numberOfKernelParameters = self.R + 1
        else:
            raise RuntimeError('Unrecognised kernel type')

        self.lowerBound = -np.inf  # Lower bound

        self.numberofBatchesPerEpoch = int(np.ceil(np.float32(self.N) / self.B))
        numPad = self.numberofBatchesPerEpoch * self.B - self.N

        self.batchStream = srng.permutation(n=self.N)
        self.padStream   = srng.choice(size=(numPad,), a=self.N,
                                       replace=False, p=None, ndim=None, dtype='int32')

        self.batchStream.name = 'batchStream'
        self.padStream.name = 'padStream'

        self.iterator = th.shared(0)
        self.iterator.name = 'iterator'

        self.allBatches = T.reshape(T.concatenate((self.batchStream, self.padStream)), [self.numberofBatchesPerEpoch, self.B])
        self.currentBatch = T.flatten(self.allBatches[self.iterator, :])

        self.allBatches.name = 'allBatches'
        self.currentBatch.name = 'currentBatch'

        self.y_miniBatch = self.y[self.currentBatch, :]
        self.y_miniBatch.name = 'y_miniBatch'

        self.jitterDefault = np.float64(0.0001)
        self.jitterGrowthFactor = np.float64(1.1)
        self.jitter = th.shared(np.asarray(self.jitterDefault, dtype='float64'), name='jitter')

        kfactory = kernelFactory(kernelType)

        # kernel parameters
        self.log_theta = sharedZeroMatrix(1, self.numberOfKernelParameters, 'log_theta', broadcastable=(True,False)) # parameters of Kuu, Kuf, Kff
        self.log_omega = sharedZeroMatrix(1, self.numberOfKernelParameters, 'log_omega', broadcastable=(True,False)) # parameters of Kuu, Kuf, Kff
        self.log_gamma = sharedZeroMatrix(1, self.numberOfKernelParameters, 'log_gamma', broadcastable=(True,False)) # parameters of Kuu, Kuf, Kff

        # Random variables
        self.xi    = srng.normal(size=(self.B, self.R), avg=0.0, std=1.0, ndim=None)
        self.alpha = srng.normal(size=(self.M, self.Q), avg=0.0, std=1.0, ndim=None)
        self.beta  = srng.normal(size=(self.B, self.Q), avg=0.0, std=1.0, ndim=None)
        self.xi.name    = 'xi'
        self.alpha.name = 'alpha'
        self.beta.name  = 'beta'

        self.sample_xi    = th.function([], self.xi)
        self.sample_alpha = th.function([], self.alpha)
        self.sample_beta  = th.function([], self.beta)

        self.sample_batchStream = th.function([], self.batchStream)
        self.sample_padStream   = th.function([], self.padStream)

        self.getCurrentBatch = th.function([], self.currentBatch, no_default_updates=True)

        # Compute parameters of q(X)
        if self.encoderType_qX == 'FreeForm1' or self.encoderType_qX == 'FreeForm2':
            # Have a normal variational distribution over location of latent co-ordinates

            self.phi_full = sharedZeroMatrix(self.N, self.R, 'phi_full')
            self.phi = self.phi_full[self.currentBatch, :]
            self.phi.name = 'phi'

            if encoderType_qX == 'FreeForm1':

                self.Phi_full_sqrt = sharedZeroMatrix(self.N, self.N, 'Phi_full_sqrt')

                Phi_batch_sqrt = self.Phi_full_sqrt[self.currentBatch][:, self.currentBatch]
                Phi_batch_sqrt.name = 'Phi_batch_sqrt'

                self.Phi = dot(Phi_batch_sqrt, Phi_batch_sqrt.T, 'Phi')

                self.cPhi, _, self.logDetPhi = cholInvLogDet(self.Phi, self.B, 0)

                self.qX_vars = [self.Phi_full_sqrt, self.phi_full]

            else:

                self.Phi_full_logdiag = sharedZeroArray(self.N, 'Phi_full_logdiag')

                Phi_batch_logdiag = self.Phi_full_logdiag[self.currentBatch]
                Phi_batch_logdiag.name = 'Phi_batch_logdiag'

                self.Phi, self.cPhi, _, self.logDetPhi \
                    = diagCholInvLogDet_fromLogDiag(Phi_batch_logdiag, 'Phi')

                self.qX_vars = [self.Phi_full_logdiag, self.phi_full]

        elif self.encoderType_qX == 'MLP':

            # Auto encode
            self.W1_qX = sharedZeroMatrix(self.H, self.P, 'W1_qX')
            self.W2_qX = sharedZeroMatrix(self.R, self.H, 'W2_qX')
            self.W3_qX = sharedZeroMatrix(1, self.H, 'W3_qX')
            self.b1_qX = sharedZeroVector(self.H, 'b1_qX', broadcastable=(False, True))
            self.b2_qX = sharedZeroVector(self.R, 'b2_qX', broadcastable=(False, True))
            self.b3_qX = sharedZeroVector(1, 'b3_qX', broadcastable=(False, True))

            # [HxB] = softplus( [HxP] . [BxP]^T + repmat([Hx1],[1,B]) )
            h_qX = softplus(plus(dot(self.W1_qX, self.y_miniBatch.T), self.b1_qX), 'h_qX' )
            # [RxB] = sigmoid( [RxH] . [HxB] + repmat([Rx1],[1,B]) )
            mu_qX = plus(dot(self.W2_qX, h_qX), self.b2_qX, 'mu_qX')
            # [1xB] = 0.5 * ( [1xH] . [HxB] + repmat([1x1],[1,B]) )
            log_sigma_qX = mul( 0.5, plus(dot(self.W3_qX, h_qX), self.b3_qX), 'log_sigma_qX')

            self.phi  = mu_qX.T  # [BxR]
            self.Phi, self.cPhi, self.iPhi,self.logDetPhi \
                = diagCholInvLogDet_fromLogDiag(log_sigma_qX, 'Phi')

            self.qX_vars = [self.W1_qX, self.W2_qX, self.W3_qX, self.b1_qX, self.b2_qX, self.b3_qX]

        elif self.encoderType_qX == 'Kernel':

            # Draw the latent coordinates from a GP with data co-ordinates
            self.Phi = kfactory.kernel(self.y_miniBatch, None, self.log_gamma, 'Phi')
            self.phi = sharedZeroMatrix(self.B, self.R, 'phi')
            (self.cPhi, self.iPhi, self.logDetPhi) = cholInvLogDet(self.Phi, self.B, self.jitter)

            self.qX_vars = [self.log_gamma]

        else:
            raise RuntimeError('Unrecognised encoding for q(X): ' + self.encoderType_qX)

        # Variational distribution q(u)
        self.kappa = sharedZeroMatrix(self.M, self.Q, 'kappa')
        self.Kappa_sqrt = sharedZeroMatrix(self.M, self.M, 'Kappa_sqrt')
        self.Kappa = dot(self.Kappa_sqrt, self.Kappa_sqrt.T, 'Kappa')

        (self.cKappa, self.iKappa, self.logDetKappa) \
                    = cholInvLogDet(self.Kappa, self.M, 0)
        self.qu_vars = [self.Kappa_sqrt, self.kappa]

        # Calculate latent co-ordinates Xf
        # [BxR]  = [BxR] + [BxB] . [BxR]
        self.Xz = plus( self.phi, dot(self.cPhi, self.xi), 'Xf' )
        # Inducing points co-ordinates
        self.Xu = sharedZeroMatrix(self.M, self.R, 'Xu')

        # Kernels
        self.Kzz = kfactory.kernel(self.Xz, None,    self.log_theta, 'Kff')
        self.Kuu = kfactory.kernel(self.Xu, None,    self.log_theta, 'Kuu')
        self.Kzu = kfactory.kernel(self.Xz, self.Xu, self.log_theta, 'Kfu')
        self.cKuu, self.iKuu, self.logDetKuu = cholInvLogDet(self.Kuu, self.M, self.jitter)

        # Variational distribution
        # A has dims [BxM] = [BxM] . [MxM]
        self.A = dot(self.Kzu, self.iKuu, 'A')
        # L is the covariance of conditional distribution q(z|u,Xf)
        self.C = minus( self.Kzz, dot(self.A, self.Kzu.T), 'C')
        self.cC, self.iC, self.logDetC = cholInvLogDet(self.C, self.B, self.jitter)

        # Sample u_q from q(u_q) = N(u_q; kappa_q, Kappa )  [MxQ]
        self.u  = plus(self.kappa, (dot(self.cKappa, self.alpha)), 'u')
        # compute mean of z [QxB]
        # [BxQ] = [BxM] * [MxQ]
        self.mu = dot(self.A, self.u, 'mu')
        # Sample f from q(f|u,X) = N( mu_q, C )
        # [BxQ] =
        self.z  = plus(self.mu, (dot(self.cC, self.beta)), 'z')

        self.qz_vars = [self.log_theta]

        self.iUpsilon = plus(self.iKappa, dot(self.A.T, dot(self.iC, self.A) ), 'iUpsilon')
        _, self.Upsilon, self.negLogDetUpsilon = cholInvLogDet(self.iUpsilon, self.M, self.jitter)

        if self.encoderType_rX == 'MLP':

            self.W1_rX = sharedZeroMatrix(self.H, self.Q+self.P, 'W1_rX')
            self.W2_rX = sharedZeroMatrix(self.R, self.H, 'W2_rX')
            self.W3_rX = sharedZeroMatrix(self.R, self.H, 'W3_rX')
            self.b1_rX = sharedZeroVector(self.H, 'b1_rX', broadcastable=(False, True))
            self.b2_rX = sharedZeroVector(self.R, 'b2_rX', broadcastable=(False, True))
            self.b3_rX = sharedZeroVector(self.R, 'b3_rX', broadcastable=(False, True))

            # [HxB] = softplus( [Hx(Q+P)] . [(Q+P)xB] + repmat([Hx1], [1,B]) )
            h_rX = softplus(plus(dot(self.W1_rX, T.concatenate((self.z.T, self.y_miniBatch.T))), self.b1_rX), 'h_rX')
            # [RxB] = softplus( [RxH] . [HxB] + repmat([Rx1], [1,B]) )
            mu_rX = plus(dot(self.W2_rX, h_rX), self.b2_rX, 'mu_rX')
            # [RxB] = 0.5*( [RxH] . [HxB] + repmat([Rx1], [1,B]) )
            log_sigma_rX = mul( 0.5, plus(dot(self.W3_rX, h_rX), self.b3_rX), 'log_sigma_rX')

            self.tau = mu_rX.T

            # Diagonal optimisation of Tau
            self.Tau_isDiagonal = True
            self.Tau = T.reshape(log_sigma_rX, [self.B * self.R, 1])
            self.logDetTau = T.sum(log_sigma_rX)
            self.Tau.name = 'Tau'
            self.logDetTau.name = 'logDetTau'

            self.rX_vars = [self.W1_rX, self.W2_rX, self.W3_rX, self.b1_rX, self.b2_rX, self.b3_rX]

        elif self.encoderType_rX == 'Kernel':

            self.tau = sharedZeroMatrix(self.B, self.R, 'tau')

            # Tau_r [BxB] = kernel( [[BxQ]^T,[BxP]^T].T )
            Tau_r = kfactory.kernel(T.concatenate((self.z.T, self.y_miniBatch.T)).T, None, self.log_omega, 'Tau_r')
            (cTau_r, iTau_r, logDetTau_r) = cholInvLogDet(Tau_r, self.B, self.jitter)

            # self.Tau  = slinalg.kron(T.eye(self.R), Tau_r)
            self.cTau = slinalg.kron(cTau_r, T.eye(self.R))
            self.iTau = slinalg.kron(iTau_r, T.eye(self.R))

            self.logDetTau = logDetTau_r * self.R
            self.tau.name  = 'tau'
            # self.Tau.name  = 'Tau'
            self.cTau.name = 'cTau'
            self.iTau.name = 'iTau'
            self.logDetTau.name = 'logDetTau'

            self.Tau_isDiagonal = False
            self.rX_vars = [self.log_omega]

        else:
            raise RuntimeError('Unrecognised encoding for r(X|z)')

        # Gradient variables - should be all the th.shared variables
        # We always want to optimise these variables
        if self.Xu_optimise:
            self.gradientVariables = [self.Xu]
        else:
            self.gradientVariables = []

        self.gradientVariables.extend(self.qu_vars)
        self.gradientVariables.extend(self.qz_vars)
        self.gradientVariables.extend(self.qX_vars)
        self.gradientVariables.extend(self.rX_vars)

        self.lowerBounds = []

        self.condKappa = myCond()(self.Kappa)
        self.condKappa.name = 'condKappa'
        self.Kappa_conditionNumber = th.function([], self.condKappa, no_default_updates=True)

        self.condKuu = myCond()(self.Kuu)
        self.condKuu.name = 'condKuu'
        self.Kuu_conditionNumber = th.function([], self.condKuu, no_default_updates=True)

        self.condC = myCond()(self.C)
        self.condC.name = 'condC'
        self.C_conditionNumber = th.function([], self.condC, no_default_updates=True)

        self.condUpsilon = myCond()(self.Upsilon)
        self.condUpsilon.name = 'condUpsilon'
        self.Upsilon_conditionNumber = th.function([], self.condUpsilon, no_default_updates=True)

        self.Xz_get_value = th.function([], self.Xz, no_default_updates=True)
コード例 #15
0
def start(clientID,
          quads,
          targets,
          speed,
          proxs,
          path,
          endpos,
          leadfoll=False):
    """
    Boids model main program
    :param clientID: ID of the VRep connection
    :param quads: quadrotor handles
    :param targets: quadrotor target handles
    :param speed: speed of quadrotors
    :param proxs: proximity sensor handles
    :param path: quadrotor path coordinates
    :param endpos: ending position of quadrotors
    :param leadfoll: True - leader/followers mode, False - all boids following path (default)
    """

    # definition of constants
    quadsNum = len(quads)  # number of quadrotors
    viewRange = 3  # view range of quadrotors
    smp = 0.2  # sampling period
    kS = [0.30,
          2.0]  # separation constants [multiplication const, power const]
    kC = [0.30, 0.0]  # cohesion constants [multiplication const, power const]
    kA = [0.00, 0.0]  # alignment constants [multiplication const, power const]
    kD = [speed,
          1.0]  # path following constants [multiplication const, power const]
    kO = [0.20, 2.0
          ]  # obstacle avoidance constants [multiplication const, power const]

    # data stream init
    for i in range(quadsNum):
        vrep.simxGetObjectPosition(clientID, quads[i], -1,
                                   vrep.simx_opmode_streaming)
        vrep.simxGetObjectVelocity(clientID, quads[i],
                                   vrep.simx_opmode_streaming)
        vrep.simxReadProximitySensor(clientID, proxs[i],
                                     vrep.simx_opmode_streaming)

    # variables init
    position = [[0 for _ in range(3)]
                for _ in range(quadsNum)]  # position of quadrotors
    velocity = [[0 for _ in range(3)]
                for _ in range(quadsNum)]  # velocity of quadrotors
    closest = [[0 for _ in range(3)]
               for _ in range(quadsNum)]  # coords of closest obstacle to quads
    visibleQuads = [[0 for _ in range(quadsNum)]
                    for _ in range(quadsNum)]  # visible quadrotors
    individualTarget = [
        0
    ] * quadsNum  # current waypoint index for each quadrotor

    # get closest boid to starting point
    leader = 0
    _, tmp = vrep.simxGetObjectPosition(clientID, quads[0], -1,
                                        vrep.simx_opmode_buffer)
    dist = ut.norm(ut.sub(path[1], tmp))
    for i in range(1, quadsNum):
        _, tmp = vrep.simxGetObjectPosition(clientID, quads[i], -1,
                                            vrep.simx_opmode_buffer)
        nrm = ut.norm(ut.sub(path[1], tmp))
        if nrm < dist:
            dist = nrm
            leader = i

    # main boids program
    t1 = 0
    t2 = 0.0
    finished = [False] * quadsNum
    count = 0
    counting = False
    file = open('data.txt', 'wt', encoding='utf-8')
    while vrep.simxGetConnectionId(clientID) != -1:
        time.sleep(smp)

        separation = [[0 for _ in range(3)]
                      for _ in range(quadsNum)]  # separation force
        cohesion = [[0 for _ in range(3)]
                    for _ in range(quadsNum)]  # cohesion force
        alignment = [[0 for _ in range(3)]
                     for _ in range(quadsNum)]  # alignment force
        destination = [[0 for _ in range(3)]
                       for _ in range(quadsNum)]  # path following force
        avoidance = [[0 for _ in range(3)]
                     for _ in range(quadsNum)]  # obstacle avoidance force
        output = [[0 for _ in range(3)]
                  for _ in range(quadsNum)]  # output force

        # check if all quads finished
        if counting:
            if count >= 0:
                file.close()
                return (t2 - t1) / 1000
            count += 1
        else:
            for i in range(quadsNum):
                nrm = ut.norm(ut.sub(position[i][0:2], path[-1][0:2]))
                if nrm < 2 and not finished[i]:
                    finished[i] = True
                    print('Quad #' + str(i) + ' finished in ' +
                          str((vrep.simxGetLastCmdTime(clientID) - t1) /
                              1000) + 's')
                    if (leadfoll and finished[leader]) or (
                            not leadfoll and all(_ for _ in finished)):
                        counting = True
                        t2 = vrep.simxGetLastCmdTime(clientID)
                        if endpos is None:
                            file.close()
                            return (t2 - t1) / 1000

        # read data from VRep
        for i in range(quadsNum):
            _, position[i] = vrep.simxGetObjectPosition(
                clientID, quads[i], -1, vrep.simx_opmode_buffer)
            _, velocity[i], _ = vrep.simxGetObjectVelocity(
                clientID, quads[i], vrep.simx_opmode_buffer)
            _, res, closest[i], _, _ = vrep.simxReadProximitySensor(
                clientID, proxs[i], vrep.simx_opmode_buffer)
            if not res:
                closest[i] = [0, 0, 0]
            closest[i][2] = 0

        # write into data file
        ct = vrep.simxGetLastCmdTime(clientID)
        file.write(str(ct))
        for i in range(quadsNum):
            file.write(' ' + str(position[i][0]) + ' ' + str(position[i][1]) +
                       ' ' + str(position[i][2]))
            file.write(' ' + str(velocity[i][0]) + ' ' + str(velocity[i][1]) +
                       ' ' + str(velocity[i][2]))
            file.write(' ' + str(closest[i][0]) + ' ' + str(closest[i][1]))
        file.write('\n')

        # compute visible quadrotors
        for i in range(quadsNum):
            for j in range(quadsNum):
                if i != j:
                    temp = ut.sub(position[i], position[j])
                    if ut.norm(temp) < viewRange:
                        visibleQuads[i][j] = 1
                    else:
                        visibleQuads[i][j] = 0

        for i in range(quadsNum):
            # compute separation force
            for j in range(quadsNum):
                if i != j and visibleQuads[i][j] == 1:
                    temp = ut.sub(position[i], position[j])
                    nrm = ut.norm(temp)
                    if nrm != 0:
                        temp = ut.mul(temp, kS[0] / (nrm**kS[1]))
                        separation[i] = ut.add(separation[i], temp)

            # compute cohesion and alignment forces
            center = [0, 0, 0]  # center of the swarm
            if sum(visibleQuads[i]) != 0:
                for j in range(quadsNum):
                    if i != j and visibleQuads[i][j] == 1:
                        temp = ut.mul(position[j], 1 / sum(visibleQuads[i]))
                        center = ut.add(center, temp)
                        temp = ut.mul(velocity[j], 1 / sum(visibleQuads[i]))
                        alignment[i] = ut.add(alignment[i], temp)
                cohesion[i] = ut.sub(center, position[i])
            nrm = ut.norm(cohesion[i])
            if nrm != 0:
                cohesion[i] = ut.mul(cohesion[i], kC[0] / (nrm**kC[1]))
            nrm = ut.norm(alignment[i])
            if nrm != 0:
                alignment[i] = ut.mul(alignment[i], kA[0] / (nrm**kA[1]))

            # compute path following force
            if not leadfoll or i == leader or counting:
                if not counting:
                    nrm = ut.norm(
                        ut.sub(position[i][0:2],
                               path[individualTarget[i]][0:2]))
                    if individualTarget[i] != 0:
                        vec1 = ut.sub(path[individualTarget[i] - 1],
                                      path[individualTarget[i]])
                        vec2 = ut.sub(position[i], path[individualTarget[i]])
                        if individualTarget[i] < len(path) - 1 and (
                                nrm <= 1
                                or ut.angle(vec1, vec2) >= math.pi / 2):
                            individualTarget[i] += 1
                            # if individualTarget[i] == 2 and min(individualTarget) == 2:
                            #     print((vrep.simxGetLastCmdTime(clientID)-tt)/1000)
                    else:
                        vec1 = ut.sub(path[individualTarget[i] + 1],
                                      path[individualTarget[i]])
                        vec2 = ut.sub(position[i], path[individualTarget[i]])
                        if nrm <= 1 or ut.angle(vec1, vec2) <= math.pi / 2:
                            individualTarget[i] += 1
                            if t1 == 0 and min(individualTarget) == 1:
                                t1 = vrep.simxGetLastCmdTime(clientID)
                            # tt = vrep.simxGetLastCmdTime(clientID)
                    destination[i] = ut.sub(path[individualTarget[i]],
                                            position[i])
                else:
                    destination[i] = ut.sub(endpos[i], position[i])
                nrm = ut.norm(destination[i])
                if nrm != 0:
                    if finished[i]:
                        destination[i] = ut.mul(destination[i], 0.1)
                    else:
                        destination[i] = ut.mul(destination[i],
                                                kD[0] / (nrm**kD[1]))

            # compute output force without obstacle avoidance
            output[i] = separation[i]
            output[i] = ut.add(output[i], cohesion[i])
            output[i] = ut.add(output[i], alignment[i])
            output[i] = ut.add(output[i], destination[i])

            # compute obstacle avoidance force
            # angle = ut.angle(closest[i], output[i])
            # if angle > math.pi/2+0.3:
            #     avoidance[i] = [0, 0, 0]
            # else:
            avoidance[i] = ut.sub([0, 0, 0], closest[i])
            nrm = ut.norm(avoidance[i])
            if nrm != 0:
                avoidance[i] = ut.mul(avoidance[i], kO[0] / (nrm**kO[1]))

            # compute output force
            output[i] = ut.add(output[i], avoidance[i])
            if position[i][2] < 0.5:
                output[i][2] = 0.05

        # export output to VRep
        for i in range(quadsNum):
            vrep.simxSetObjectPosition(clientID, targets[i], quads[i],
                                       output[i], vrep.simx_opmode_streaming)
コード例 #16
0
ファイル: day16.py プロジェクト: iKevinY/advent
            tickets.append(nums)

valid_tickets = []
part_1 = 0

for ticket in tickets[1:]:
    for n in ticket:
        for a, b, c, d in fields.values():
            if a <= n <= b or c <= n <= d:
                break

        else:
            part_1 += n
            break
    else:
        valid_tickets.append(ticket)

print "Part 1:", part_1

poss = defaultdict(set)

for i, col in enumerate(transposed(valid_tickets)):
    for field, (a, b, c, d) in fields.items():
        if all(a <= n <= b or c <= n <= d for n in col):
            poss[field].add(i)

resolved = resolve_mapping(poss)

departures = [resolved[field] for field in resolved if 'departure' in field]
print "Part 2:", mul(tickets[0][i] for i in departures)
コード例 #17
0
def voro_start(clientID, obstacles, border):
    """
    Creates a Voronoi diagram around obstacles using vtk_voro
    :param clientID: ID of the VRep connection
    :param obstacles: list of obstacle handles
    :param border: scene border [width, height]
    :return: list of graph vertices, indices of edge vertices, gaps for each edge
    """

    border = [[-border[0] / 2, -border[1] / 2],
              [-border[0] / 2, border[1] / 2], [border[0] / 2, border[1] / 2],
              [border[0] / 2, -border[1] / 2]]
    obsNum = len(obstacles)
    output = []
    for i in range(obsNum):
        # get data from V-Rep
        _, position = vrep.simxGetObjectPosition(clientID, obstacles[i], -1,
                                                 vrep.simx_opmode_oneshot_wait)
        _, orientation = vrep.simxGetObjectOrientation(
            clientID, obstacles[i], -1, vrep.simx_opmode_oneshot_wait)
        orientation = orientation[2]

        _, minX = vrep.simxGetObjectFloatParameter(
            clientID, obstacles[i], 15, vrep.simx_opmode_oneshot_wait)
        _, minY = vrep.simxGetObjectFloatParameter(
            clientID, obstacles[i], 16, vrep.simx_opmode_oneshot_wait)
        _, maxX = vrep.simxGetObjectFloatParameter(
            clientID, obstacles[i], 18, vrep.simx_opmode_oneshot_wait)
        _, maxY = vrep.simxGetObjectFloatParameter(
            clientID, obstacles[i], 19, vrep.simx_opmode_oneshot_wait)

        corners = [[minX, minY], [minX, maxY], [maxX, minY], [maxX, maxY]]

        # compute bounding box corners for angled object
        center = ut.mul(ut.add(corners[0], corners[3]), 0.5)
        radius = ut.norm(ut.sub(corners[0], center))
        zero = ut.mul(ut.add(corners[0], corners[2]), 0.5)

        angle = ut.angle(ut.sub(corners[0], center), ut.sub(
            zero, center)) + orientation + math.pi / 2
        corners[0][0] = center[0] + radius * math.cos(angle)
        corners[0][1] = center[1] + radius * math.sin(angle)

        angle = ut.angle(ut.sub(corners[1], center), ut.sub(
            zero, center)) + orientation + math.pi / 2
        corners[1][0] = center[0] + radius * math.cos(angle)
        corners[1][1] = center[1] + radius * math.sin(angle)

        vec = ut.sub(center, corners[0])
        corners[2] = ut.add(center, vec)

        vec = ut.sub(center, corners[1])
        corners[3] = ut.add(center, vec)

        # move to obstacle position and convert to millimeters
        for j in range(4):
            corners[j] = ut.add(position[0:2], corners[j])
            for k in range(2):
                corners[j][k] = int(corners[j][k] * 1000)
        output.append(corners)

    # merge overlapping obstacles
    ignore = []
    merged = []
    for i in range(obsNum):
        if i not in ignore:
            tmp, visited = ut.merge_obstacles(i, output, ignore)
            ignore = ignore + visited
            for j in range(len(tmp)):
                for k in range(2):
                    tmp[j][k] = int(tmp[j][k])
            merged.append(tmp)

    # write to file to be used by vtk_voro
    file = open('voro_data.txt', 'wt', encoding='utf-8')
    file.write('[BORDER]\n' + str(border[0][0]) + ' ' + str(border[0][1]) +
               '\n' + str(border[1][0]) + ' ' + str(border[1][1]) + '\n' +
               str(border[2][0]) + ' ' + str(border[2][1]) + '\n' +
               str(border[3][0]) + ' ' + str(border[3][1]) + '\n\n')
    for i in range(len(merged)):
        file.write('[OBSTACLE]\n')
        for j in range(len(merged[i])):
            file.write(
                str(merged[i][j][0]) + ' ' + str(merged[i][j][1]) + '\n')
        file.write('\n')
    file.close()

    # execute vtk_voro
    command = "start /wait cmd /c cd " + os.getcwd(
    ) + " && .\\vtk_voro\\build\\Debug\\vtk_voro.exe voro_data.txt"
    os.system(command)

    # read vtk_voro output
    vertices = []
    edges = []
    mode = True
    with open('voro_output.txt', 'rt', encoding='utf-8') as f:
        for line in f:
            tmp = line.split()
            if tmp[0] == "---":
                mode = False
                continue
            if mode:
                for i in range(len(tmp)):
                    tmp[i] = float(tmp[i]) / 1000
                vertices.append(tmp)
            else:
                for i in range(len(tmp)):
                    tmp[i] = int(tmp[i])
                edges.append(tmp)

    # sample the obstacles for computing gaps
    merged.append(border)
    for i in range(len(merged)):
        for j in range(len(merged[i])):
            for k in range(len(merged[i][j])):
                merged[i][j][k] /= 1000

    for i in range(len(merged)):
        size = len(merged[i])
        for j in range(size):
            if j == size - 1:
                p0 = merged[i][j]
                p1 = merged[i][0]
            else:
                p0 = merged[i][j]
                p1 = merged[i][j + 1]
            vec = ut.sub(p1, p0)
            length = ut.norm(vec)
            vec = ut.mul(vec, 1 / length)

            for k in range(int(length / 0.1)):
                merged[i].append(ut.add(p0, ut.mul(vec, (k + 1) * 0.1)))

    # find size of the gap for each edge
    gaps = []
    for edge in edges:
        p0 = vertices[edge[0]]
        p1 = vertices[edge[1]]
        center = ut.mul(ut.add(p0, p1), 0.5)
        vec = ut.sub(p1, p0)
        norm = [-vec[1], vec[0]]
        length = ut.norm(vec)
        line = [norm[0], norm[1], -norm[0] * p0[0] - norm[1] * p0[1]]
        normal = [vec[0], vec[1], -vec[0] * center[0] - vec[1] * center[1]]

        mindist = [sys.maxsize, sys.maxsize]
        closest = [None, None]
        for obstacle in merged:
            for point in obstacle:
                if ut.point_line(point, normal) <= length / 2 + 0.01:
                    dist = ut.point_line(point, line)
                    if line[0] * point[0] + line[1] * point[1] + line[2] > 0:
                        if dist < mindist[0]:
                            mindist[0] = dist
                            closest[0] = point
                    elif dist < mindist[1]:
                        mindist[1] = dist
                        closest[1] = point
        if None in closest:
            gaps.append(None)
        else:
            gaps.append(ut.norm(ut.sub(closest[0], closest[1])))

    return vertices, edges, gaps
コード例 #18
0
# Modules

# modules means files
# packages means folders

import utils # importing utils.py file from the same directory
print(utils.mul(2,3)) # imported files can be used this way

# Different ways to import

import module
from package.subpackage import module
from package.module import func1, func2 # this way we can know what exactly we've imported
from package import *

# =================================================================================

# What is
if __name == __main__

        #^^^^^ notice double equals

# __main__ is the name of the file which is being executed

# every module file gets __name__ equals to the file name

# to ensure, a block of code is only executed if the executing file is the __main__ file, we use the condition

if __name == __main__:
    # rest code goes here
コード例 #19
0
ファイル: day16.py プロジェクト: iKevinY/advent
def parse_packets(bits, num_subpackets=None, bit_length=None, depth=0, quiet=False):
    """
    Given a bitstring `bits`, parses `num_subpackets` subpackets, and returns ([packets], bit_length, version).

    If the argument `num_subpackets` is given, this dictates the number of packets
    to parse from `bits` before returning.

    If the argument `bit_length` is given, this dictates how many bits to process
    before returning however many packets the parser got through.

    If `quiet` is False, the parser will print the operator and values processed,
    with various nesting levels based on `depth` (incremented per recursive call).
    """

    i = 0        # current pointer into `bits`
    subs = []    # list of subpackets currently parsed
    version = 0  # the version so far (?)

    try:
        while True:
            # Standard Packet Header

            # Read 3-bit version
            version += int(bits[i:i+3], 2)
            i += 3

            # Read 3-bit type ID
            ttype = int(bits[i:i+3], 2)
            i += 3

            # Packets with type ID 4 represent a literal value.
            if ttype == 4:
                lit = ''

                # Read groups of 5 bits until the final group
                # (prefixed 0) is seen. Then stop reading.
                while True:
                    group = bits[i:i+5]
                    i += 5
                    lit += group[1:]
                    if group[0] == '0':
                        break

                try:
                    lit = int(lit, 2)
                    subs.append(lit)
                except Exception:
                    print "bad literal, moving on"

            # Otherwise, process an operator.
            else:
                # Operator packet mode is based on its 1-bit length type ID.
                len_type_id = bits[i]
                i += 1

                # Specifies the bit length of contained subpackets.
                if len_type_id == '0':
                    sub_len = int(bits[i:i+15], 2)
                    i += 15
                    subpackets, new_i, new_version = parse_packets(bits[i:i+sub_len], bit_length=sub_len, depth=depth + 1, quiet=quiet)

                    i += new_i
                    version += new_version

                # Specifies total number of subpackets.
                elif len_type_id == '1':
                    sub_packets = int(bits[i:i+11], 2)
                    i += 11
                    subpackets, new_i, new_version = parse_packets(bits[i:], num_subpackets=sub_packets, depth=depth+1, quiet=quiet)

                    i += new_i
                    version += new_version
                else:
                    print "Unknown length type ID"

                # Process operators
                if not quiet:
                    print " " * depth, OPERATORS[ttype], subpackets

                if ttype == 0:
                    subs.append(sum(subpackets))
                elif ttype == 1:
                    subs.append(mul(subpackets))
                elif ttype == 2:
                    subs.append(min(subpackets))
                elif ttype == 3:
                    subs.append(max(subpackets))
                elif ttype == 5:
                    subs.append(1 if subpackets[0] > subpackets[1] else 0)
                elif ttype == 6:
                    subs.append(1 if subpackets[0] < subpackets[1] else 0)
                elif ttype == 7:
                    subs.append(1 if subpackets[0] == subpackets[1] else 0)

            # Check exit conditions for loop.
            if num_subpackets is not None and len(subs) == num_subpackets:
                # IMPORTANT: return i and not len(i); it is not an invariant
                # that we read the full `bits`, since there may have been
                # some trailing data that we ignore since it falls out of
                # the range of `num_subpackets` subpackets.
                return subs, i, version
            elif bit_length is not None and i > bit_length:
                return subs, len(bits), version
            elif i >= len(bits):
                return subs, len(bits), version

    except Exception as e:
        print "Unexpected parsing error:", e
        return subs, i, version
コード例 #20
0
def start(clientID, quads, targets, speed, proxs, path, leadfoll=False):
    """
    Boids model program for experimental graph edges evaluation
    :param clientID: ID of the VRep connection
    :param quads: quadrotor handles
    :param targets: quadrotor target handles
    :param speed: speed of quadrotors
    :param proxs: proximity sensor handles
    :param path: quadrotor path coordinates
    :param leadfoll: True - leader/followers mode, False - all boids following path (default)
    """

    # definition of constants
    quadsNum = len(quads)  # number of quadrotors
    viewRange = 3  # view range of quadrotors
    smp = 0.2  # sampling period
    kS = [0.30,
          2.0]  # separation constants [multiplication const, power const]
    kC = [0.30, 0.0]  # cohesion constants [multiplication const, power const]
    kA = [0.00, 0.0]  # alignment constants [multiplication const, power const]
    kD = [speed,
          1.0]  # path following constants [multiplication const, power const]
    kO = [0.20, 2.0
          ]  # obstacle avoidance constants [multiplication const, power const]

    # data stream init
    for i in range(quadsNum):
        vrep.simxGetObjectPosition(clientID, quads[i], -1,
                                   vrep.simx_opmode_streaming)
        vrep.simxGetObjectVelocity(clientID, quads[i],
                                   vrep.simx_opmode_streaming)
        vrep.simxReadProximitySensor(clientID, proxs[i],
                                     vrep.simx_opmode_streaming)

    # variables init
    position = [[0 for _ in range(3)]
                for _ in range(quadsNum)]  # position of quadrotors
    velocity = [[0 for _ in range(3)]
                for _ in range(quadsNum)]  # velocity of quadrotors
    closest = [[0 for _ in range(3)]
               for _ in range(quadsNum)]  # coords of closest obstacle to quads
    visibleQuads = [[0 for _ in range(quadsNum)]
                    for _ in range(quadsNum)]  # visible quadrotors
    individualTarget = [
        0
    ] * quadsNum  # current waypoint index for each quadrotor

    # get closest boid to starting point
    leader = 0
    _, tmp = vrep.simxGetObjectPosition(clientID, quads[0], -1,
                                        vrep.simx_opmode_buffer)
    dist = ut.norm(ut.sub(path[1], tmp))
    for i in range(1, quadsNum):
        _, tmp = vrep.simxGetObjectPosition(clientID, quads[i], -1,
                                            vrep.simx_opmode_buffer)
        nrm = ut.norm(ut.sub(path[1], tmp))
        if nrm < dist:
            dist = nrm
            leader = i

    # main boids program
    print('Evaluating ' + str(len(path)) + ' edges:')
    number = 0
    cnt = [0] * quadsNum
    finished = [0] * len(path)
    last = [False] * quadsNum
    end = False
    t1 = vrep.simxGetLastCmdTime(clientID)
    ec = []
    while vrep.simxGetConnectionId(clientID) != -1:
        time.sleep(smp)

        separation = [[0 for _ in range(3)]
                      for _ in range(quadsNum)]  # separation force
        cohesion = [[0 for _ in range(3)]
                    for _ in range(quadsNum)]  # cohesion force
        alignment = [[0 for _ in range(3)]
                     for _ in range(quadsNum)]  # alignment force
        destination = [[0 for _ in range(3)]
                       for _ in range(quadsNum)]  # path following force
        avoidance = [[0 for _ in range(3)]
                     for _ in range(quadsNum)]  # obstacle avoidance force
        output = [[0 for _ in range(3)]
                  for _ in range(quadsNum)]  # output force

        # read data from VRep
        for i in range(quadsNum):
            _, position[i] = vrep.simxGetObjectPosition(
                clientID, quads[i], -1, vrep.simx_opmode_buffer)
            _, velocity[i], _ = vrep.simxGetObjectVelocity(
                clientID, quads[i], vrep.simx_opmode_buffer)
            _, res, closest[i], _, _ = vrep.simxReadProximitySensor(
                clientID, proxs[i], vrep.simx_opmode_buffer)
            if not res:
                closest[i] = [0, 0, 0]
            closest[i][2] = 0

        # compute visible quadrotors
        for i in range(quadsNum):
            for j in range(quadsNum):
                if i != j:
                    temp = ut.sub(position[i], position[j])
                    if ut.norm(temp) < viewRange:
                        visibleQuads[i][j] = 1
                    else:
                        visibleQuads[i][j] = 0

        for i in range(quadsNum):
            # compute separation force
            for j in range(quadsNum):
                if i != j and visibleQuads[i][j] == 1:
                    temp = ut.sub(position[i], position[j])
                    nrm = ut.norm(temp)
                    if nrm != 0:
                        temp = ut.mul(temp, kS[0] / (nrm**kS[1]))
                        separation[i] = ut.add(separation[i], temp)

            # compute cohesion and alignment forces
            center = [0, 0, 0]  # center of the swarm
            if sum(visibleQuads[i]) != 0:
                for j in range(quadsNum):
                    if i != j and visibleQuads[i][j] == 1:
                        temp = ut.mul(position[j], 1 / sum(visibleQuads[i]))
                        center = ut.add(center, temp)
                        temp = ut.mul(velocity[j], 1 / sum(visibleQuads[i]))
                        alignment[i] = ut.add(alignment[i], temp)
                cohesion[i] = ut.sub(center, position[i])
            nrm = ut.norm(cohesion[i])
            if nrm != 0:
                cohesion[i] = ut.mul(cohesion[i], kC[0] / (nrm**kC[1]))
            nrm = ut.norm(alignment[i])
            if nrm != 0:
                alignment[i] = ut.mul(alignment[i], kA[0] / (nrm**kA[1]))

            # compute path following force
            check = False
            if not leadfoll or i == leader or end:
                nrm = ut.norm(
                    ut.sub(position[i][0:2], path[individualTarget[i]][0:2]))
                if end:
                    if finished[i] == 0 and nrm < 2:
                        finished[i] += 1
                    if sum(finished) == quadsNum:
                        return ec
                else:
                    if individualTarget[i] != 0:
                        if not last[i]:
                            tmp = min(individualTarget)
                            if individualTarget[i] == tmp and tmp != max(
                                    individualTarget):
                                cnt[i] += 1
                            vec1 = ut.sub(path[individualTarget[i] - 1],
                                          path[individualTarget[i]])
                            vec2 = ut.sub(position[i],
                                          path[individualTarget[i]])
                            if nrm <= 1 or cnt[i] > 30 or ut.angle(
                                    vec1, vec2) >= math.pi / 2:
                                if cnt[i] != 0:
                                    cnt = [0] * quadsNum
                                finished[individualTarget[i]] += 1
                                if leadfoll or finished[
                                        individualTarget[i]] == quadsNum:
                                    check = True
                                if individualTarget[i] < len(path) - 1:
                                    individualTarget[i] += 1
                                else:
                                    last[i] = True
                    else:
                        vec1 = ut.sub(path[individualTarget[i] + 1],
                                      path[individualTarget[i]])
                        vec2 = ut.sub(position[i], path[individualTarget[i]])
                        if nrm <= 1 or ut.angle(vec1, vec2) <= math.pi / 2:
                            finished[individualTarget[i]] += 1
                            if leadfoll or finished[
                                    individualTarget[i]] == quadsNum:
                                check = True
                            individualTarget[i] += 1
                    if check:
                        t2 = vrep.simxGetLastCmdTime(clientID)
                        ec.append((t2 - t1) / 1000)
                        print('Edge #' + str(number) + ': ' +
                              str((t2 - t1) / 1000) + 's')
                        number += 1
                        if number == len(path):
                            return ec
                        t1 = t2
                        if (leadfoll and finished[-1]
                                == 1) or finished[-1] == quadsNum:
                            end = True
                            individualTarget = [0] * quadsNum
                            finished = [0] * quadsNum
                destination[i] = ut.sub(path[individualTarget[i]], position[i])
                nrm = ut.norm(destination[i])
                if nrm != 0:
                    destination[i] = ut.mul(destination[i],
                                            kD[0] / (nrm**kD[1]))

            # compute output force without obstacle avoidance
            output[i] = separation[i]
            output[i] = ut.add(output[i], cohesion[i])
            output[i] = ut.add(output[i], alignment[i])
            output[i] = ut.add(output[i], destination[i])

            # compute obstacle avoidance force
            # angle = ut.angle(closest[i], output[i])
            # if angle > math.pi/2+0.3:
            #     avoidance[i] = [0, 0, 0]
            # else:
            avoidance[i] = ut.sub([0, 0, 0], closest[i])
            nrm = ut.norm(avoidance[i])
            if nrm != 0:
                avoidance[i] = ut.mul(avoidance[i], kO[0] / (nrm**kO[1]))

            # compute output force
            output[i] = ut.add(output[i], avoidance[i])
            if position[i][2] < 0.5:
                output[i][2] = 0.05

        # export output to VRep
        for i in range(quadsNum):
            vrep.simxSetObjectPosition(clientID, targets[i], quads[i],
                                       output[i], vrep.simx_opmode_streaming)
コード例 #21
0
 def _mul_update_impl(self, x, y, out, beta):
     """
     use a custom built Elementwise kernel specifically created for
     pre-allocated memories
     """
     utils.mul(x, y, out=out, beta=beta)
コード例 #22
0
ファイル: day20.py プロジェクト: iKevinY/advent
    for _ in range(4):
        row = ''.join(tile[0])
        piece_edges[row].add(id)
        piece_edges[row[::-1]].add(id)
        tile = rotated(tile)

# Construct graph of neighbouring pieces
graph = defaultdict(set)

for neighbouring_pieces in piece_edges.values():
    for x, y in permutations(neighbouring_pieces, 2):
        graph[x].add(y)

# Corners are the only pieces that neighbour exactly 2 other tiles.
corners = [tile for tile, neighs in graph.items() if len(neighs) == 2]
print "Corner ID product:", mul(corners)
print

# Build up the full arrangement of the tiles. Arbitrarily pick
# a corner to be the top-left, and assign its neighbours.
puzzle = {}
puzzle[0, 0] = corners[0]

# Let `m` represent the m by m arrangement of tiles.
m = int(math.sqrt(len(TILES)))

# Assign tiles for the top row and left column by choosing from the subset
# of tiles that contain either 2 or 3 neighbours (ie. edges and corners).
# Make sure to not reuse tiles since that ruins this algorithm.
edges = [tile for tile, neighs in graph.items() if len(neighs) == 3]
コード例 #23
0
def load_map(shift, enlarge, include_border, clientID):
    """
    Loads map from voro_data.txt, creates Voronoi diagram and adjusts the output
    :param shift: shift the map by [x,y]
    :param enlarge: enlarge the map
    :param include_border: include border when computing size of gaps
    :param clientID: ID of the VRep connection (only for visualization)
    :return: list of graph vertices, indices of edge vertices, gaps for each edge
    """

    # read map file
    border = []
    obstacles = [[]]
    mode = True
    idx = 0
    with open('voro_data.txt', 'rt', encoding='utf-8') as f:
        for line in f:
            tmp = line.split()
            if tmp:
                if tmp[0].lower() == '[border]':
                    mode = True
                elif tmp[0].lower() == '[obstacle]':
                    obstacles.append([])
                    idx += 1
                    mode = False
                else:
                    if mode:
                        tmp[0] = float(tmp[0]) + shift[0]
                        tmp[1] = float(tmp[1]) + shift[1]
                        for i in range(len(tmp)):
                            tmp[i] = enlarge * float(tmp[i]) / 1000
                        border.append(tmp)
                    else:
                        tmp[0] = float(tmp[0]) + shift[0]
                        tmp[1] = float(tmp[1]) + shift[1]
                        for i in range(len(tmp)):
                            tmp[i] = enlarge * float(tmp[i]) / 1000
                        obstacles[idx].append(tmp)
    del obstacles[0]

    # execute vtk_voro
    command = "start /wait cmd /c cd " + os.getcwd(
    ) + " && .\\vtk_voro\\build\\Debug\\vtk_voro.exe voro_data.txt"
    os.system(command)

    # read vtk_voro output
    vertices = []
    edges = []
    mode = True
    with open('voro_output.txt', 'rt', encoding='utf-8') as f:
        for line in f:
            tmp = line.split()
            if tmp[0] == "---":
                mode = False
                continue
            if mode:
                tmp[0] = float(tmp[0]) + shift[0]
                tmp[1] = float(tmp[1]) + shift[1]
                for i in range(len(tmp)):
                    tmp[i] = enlarge * float(tmp[i]) / 1000
                vertices.append(tmp)
            else:
                for i in range(len(tmp)):
                    tmp[i] = int(tmp[i])
                edges.append(tmp)

    # for vertex in vertices:
    #     _, tmp = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
    #     vrep.simxSetObjectPosition(clientID, tmp, -1, vertex, vrep.simx_opmode_oneshot_wait)

    # sample the obstacles for computing gaps
    if include_border:
        obstacles.append(border)

    # for obstacle in obstacles:
    #     for point in obstacle:
    #         _, tmp = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
    #         vrep.simxSetObjectPosition(clientID, tmp, -1, point, vrep.simx_opmode_oneshot_wait)

    for i in range(len(obstacles)):
        size = len(obstacles[i])
        for j in range(size):
            if j == size - 1:
                p0 = obstacles[i][j]
                p1 = obstacles[i][0]
            else:
                p0 = obstacles[i][j]
                p1 = obstacles[i][j + 1]
            vec = ut.sub(p1, p0)
            length = ut.norm(vec)
            vec = ut.mul(vec, 1 / length)

            for k in range(int(length / 0.1)):
                obstacles[i].append(ut.add(p0, ut.mul(vec, (k + 1) * 0.1)))

    # for obstacle in obstacles:
    #     for point in obstacle:
    #         _, tmp = vrep.simxCreateDummy(clientID, 0.1, None, vrep.simx_opmode_oneshot_wait)
    #         vrep.simxSetObjectPosition(clientID, tmp, -1, point, vrep.simx_opmode_oneshot_wait)

    # find size of the gap for each edge
    gaps = []
    for edge in edges:
        p0 = vertices[edge[0]]
        p1 = vertices[edge[1]]
        center = ut.mul(ut.add(p0, p1), 0.5)
        vec = ut.sub(p1, p0)
        norm = [-vec[1], vec[0]]
        length = ut.norm(vec)
        line = [norm[0], norm[1], -norm[0] * p0[0] - norm[1] * p0[1]]
        normal = [vec[0], vec[1], -vec[0] * center[0] - vec[1] * center[1]]

        mindist = [sys.maxsize, sys.maxsize]
        found = [None, None]
        for obstacle in obstacles:
            for point in obstacle:
                if ut.point_line(point, normal) <= length / 2 + 0.01:
                    dist = ut.point_line(point, line)
                    if line[0] * point[0] + line[1] * point[1] + line[2] > 0:
                        if dist < mindist[0]:
                            mindist[0] = dist
                            found[0] = True
                    elif dist < mindist[1]:
                        mindist[1] = dist
                        found[1] = True
        if None in found:
            gaps.append(None)
        else:
            gaps.append(sum(mindist))

    return vertices, edges, gaps