Solve KKT equations for the affine step
199118
def solve_kkt(Q_LU, d, G, A, S_LU, rx, rs, rz, ry):
    
    nineq, nz, neq, nBatch = get_sizes(G, A)

    invQ_rx = rx.btrisolve(*Q_LU)
    if neq > 0:
        h = torch.cat((invQ_rx.unsqueeze(1).bmm(A.transpose(1, 2)).squeeze(1) - ry,
                       invQ_rx.unsqueeze(1).bmm(G.transpose(1, 2)).squeeze(1) + rs / d - rz), 1)
    else:
        h = invQ_rx.unsqueeze(1).bmm(G.transpose(1, 2)).squeeze(1) + rs / d - rz

    w = -(h.btrisolve(*S_LU))

    g1 = -rx - w[:, neq:].unsqueeze(1).bmm(G).squeeze(1)
    if neq > 0:
        g1 -= w[:, :neq].unsqueeze(1).bmm(A).squeeze(1)
    g2 = -rs - w[:, neq:]

    dx = g1.btrisolve(*Q_LU)
    ds = g2 / d
    dz = w[:, neq:]
    dy = w[:, :neq] if neq > 0 else None

    return dx, ds, dz, dy
164521
def step(self, y, u, t, h):
		
		k1 = h * self.func(t, y, u)
		k2 = h * self.func(t + .5*h, y + .5*h*k1, u)
		k3 = h * self.func(t + .5*h, y + .5*h*k2, u)
		k4 = h * self.func(t + h, y + h*k3, u)
		return y + (k1 + 2*k2 + 2*k3 + k4) / 6.0
572163
def do_step(self, values, xy_values,coeff, width):
        """"""
        forces = {k:[] for k,i in enumerate(xy_values)}
        for (index1, value1), (index2,value2) in combinations(enumerate(xy_values),2):
            f = self.calc_2d_forces(value1[0],value1[1],value2[0],value2[1],width)
            if coeff[index1] < coeff[index2]:
                if self.b_lenght-coeff[index2]<self.b_lenght/10: #a quick and dirty solution, but works
                    forces[index1].append(f[1]) # push to left (smaller projection value)
                    forces[index2].append(f[0])
                else:
                    #all is normal
                    forces[index1].append(f[0]) # push to left (smaller projection value)
                    forces[index2].append(f[1])
            else:
                if self.b_lenght-coeff[index1]<self.b_lenght/10: #a quick and dirty solution, but works
                    forces[index1].append(f[0]) # push to left (smaller projection value)
                    forces[index2].append(f[1])
                else:
                #if all is normal
                    forces[index1].append(f[1]) # push to left (smaller projection value)
                    forces[index2].append(f[0])
        forces = {k:sum(v) for k,v in forces.items()}

        energy = sum([abs(x) for x in forces.values()])

        return [(forces[k]/10+v) for k, v in enumerate(values)], energy
181780
private DoubleMatrix1D calculateNewtonStep(DoubleMatrix2D hessX, DoubleMatrix1D gradX) throws Exception {
		final KKTSolver kktSolver = new BasicKKTSolver();
		if(isCheckKKTSolutionAccuracy()){
			kktSolver.setCheckKKTSolutionAccuracy(isCheckKKTSolutionAccuracy());
			kktSolver.setToleranceKKT(getToleranceKKT());
		}
		kktSolver.setHMatrix(hessX);
		kktSolver.setGVector(gradX);
		DoubleMatrix1D[] sol = kktSolver.solve();
		DoubleMatrix1D step = sol[0];
		return step;
	}
518588
def _backward_step(self, deltaY, deltaY_sigma2inv_rho_weightT,
                       sigma2_e, weight, mu, mu_Gamma_inv, Gamma_inv,
                       Lambda_0, Lambda_1, H):
        
        n_T = len(Gamma_inv)
        # All the terms with hat before are parameters of posterior
        # distributions of X conditioned on data from all time points,
        # whereas the ones without hat calculated by _forward_step
        # are mean and covariance of posterior of X conditioned on
        # data up to the time point.
        Gamma_inv_hat = [None] * n_T
        mu_Gamma_inv_hat = [None] * n_T
        mu_hat = [None] * n_T

        mu_hat[-1] = mu[-1].copy()
        mu_Gamma_inv_hat[-1] = mu_Gamma_inv[-1].copy()
        Gamma_inv_hat[-1] = Gamma_inv[-1].copy()

        for t in np.arange(n_T - 2, -1, -1):
            tmp = np.linalg.solve(Gamma_inv_hat[t + 1] - Gamma_inv[t + 1]
                                  + Lambda_1, H)
            Gamma_inv_hat[t] = Gamma_inv[t] + Lambda_0 - np.dot(H.T, tmp)
            mu_Gamma_inv_hat[t] = mu_Gamma_inv[t] \
                - deltaY_sigma2inv_rho_weightT[t, :] + np.dot(
                    mu_Gamma_inv_hat[t + 1] - mu_Gamma_inv[t + 1]
                    + np.dot(deltaY[t, :] / sigma2_e, weight.T), tmp)
            mu_hat[t] = np.linalg.solve(Gamma_inv_hat[t],
                                        mu_Gamma_inv_hat[t])
        return mu_hat, mu_Gamma_inv_hat, Gamma_inv_hat
565991
def solve(self, lam):
        ''''''
        s = weighted_graphtf(self.nnodes, self.y, self.weights, lam,
                             self.Dk.shape[0], self.Dk.shape[1], self.Dk.nnz,
                             self.Dk.row.astype('int32'), self.Dk.col.astype('int32'), self.Dk.data.astype('double'),
                             self.maxsteps, self.converge,
                             self.beta, self.u)
        self.steps.append(s)
        return self.beta
1344470
def xstep(self):
        
        """

        self.X = np.asarray(sl.lu_solve_AATI(self.Z, self.rho, self.SZT +
                            self.rho*(self.Y - self.U), self.lu, self.piv,),
                            dtype=self.dtype)
190026
private double[][] solveLtransposed(double[][] X) {
    for(int k = L.length - 1; k >= 0; k--) {
      final double[] Lk = L[k], Xk = X[k];
      timesEquals(Xk, 1. / Lk[k]);
      for(int i = 0; i < k; i++) {
        minusTimesEquals(X[i], Xk, Lk[i]);
      }
    }
    return X;
  }
699882
def funTransQuadF(k, s):
    
    sqrtk = np.sqrt(complex(k))
    a = np.cos(sqrtk * s)
    b = np.sin(sqrtk * s) / sqrtk
    c = -sqrtk * np.sin(sqrtk * s)
    d = np.cos(sqrtk * s)
    return np.matrix([[a.real, b.real], [c.real, d.real]], dtype=np.double)
120968
def _solve(self, x0, A, l, u, xmin, xmax):
        
        s = pips(self._costfcn, x0, A, l, u, xmin, xmax,
                 self._consfcn, self._hessfcn, self.opt)
        return s
843587
def combination_step(self):
        

        # Update t step
        tprv = self.t
        self.t = 0.5 * float(1. + np.sqrt(1. + 4. * tprv**2))

        # Update Y
        if not self.opt['FastSolve']:
            self.Yprv = self.Y.copy()
        self.Y = self.X + ((tprv - 1.) / self.t) * (self.X - self.Xprv)
1241616
def pre_factor_kkt(Q, G, A):
    
    nineq, nz, neq, _ = get_sizes(G, A)

    # S = [ A Q^{-1} A^T        A Q^{-1} G^T           ]
    #     [ G Q^{-1} A^T        G Q^{-1} G^T + D^{-1} ]

    U_Q = torch.potrf(Q)
    # partial cholesky of S matrix
    U_S = torch.zeros(neq + nineq, neq + nineq).type_as(Q)

    G_invQ_GT = torch.mm(G, torch.potrs(G.t(), U_Q))
    R = G_invQ_GT
    if neq > 0:
        invQ_AT = torch.potrs(A.t(), U_Q)
        A_invQ_AT = torch.mm(A, invQ_AT)
        G_invQ_AT = torch.mm(G, invQ_AT)

        # TODO: torch.potrf sometimes says the matrix is not PSD but
        # numpy does? I filed an issue at
        # https://github.com/pytorch/pytorch/issues/199
        try:
            U11 = torch.potrf(A_invQ_AT)
        except:
            U11 = torch.Tensor(np.linalg.cholesky(
                A_invQ_AT.cpu().numpy())).type_as(A_invQ_AT)

        # TODO: torch.trtrs is currently not implemented on the GPU
        # and we are using gesv as a workaround.
        U12 = torch.gesv(G_invQ_AT.t(), U11.t())[0]
        U_S[:neq, :neq] = U11
        U_S[:neq, neq:] = U12
        R -= torch.mm(U12.t(), U12)

    return U_Q, U_S, R
1218739
def update_variables(X, Z, U, prox_f, step_f, prox_g, step_g, L):
    
    if not hasattr(prox_g, '__iter__'):
        if prox_g is not None:
            dX = step_f/step_g * L.T.dot(L.dot(X) - Z + U)
            X[:] = prox_f(X - dX, step_f)
            LX, R, S = do_the_mm(X, step_f, Z, U, prox_g, step_g, L)
        else:
            # fall back to simple fixed-point method for f
            # see do_the_mm for normal definitions of LX,Z,R,S
            S = -X.copy()
            X[:] = prox_f(X, step_f)
            LX = X
            Z[:] = X[:]
            R = np.zeros(X.shape, dtype=X.dtype)
            S += X

    else:
        M = len(prox_g)
        dX = np.sum([step_f/step_g[i] * L[i].T.dot(L[i].dot(X) - Z[i] + U[i]) for i in range(M)], axis=0)
        X[:] = prox_f(X - dX, step_f)
        LX = [None] * M
        R = [None] * M
        S = [None] * M
        for i in range(M):
            LX[i], R[i], S[i] = do_the_mm(X, step_f, Z[i], U[i], prox_g[i], step_g[i], L[i])
    return LX, R, S
886784
def solve_t(self, Mt):
        
        if len(Mt.shape)==2:    _Mt = Mt[:, :, sp.newaxis]
        else:                   _Mt = Mt
        LMt = vei_CoR_veX(_Mt, R=self.Lr(), C=self.Lc())
        DMt = self.D()[:, :, sp.newaxis] * LMt
        WrDMtWc = vei_CoR_veX(DMt, R=self.Wr().T, C=self.Wc().T)
        ve_WrDMtWc = sp.reshape(WrDMtWc, (WrDMtWc.shape[0] * WrDMtWc.shape[1], _Mt.shape[2]), order='F')
        Hi_ve_WrDMtWc = la.cho_solve((self.H_chol(), True), ve_WrDMtWc)
        vei_HiveWrDMtWc = Hi_ve_WrDMtWc.reshape(WrDMtWc.shape, order = 'F')
        Wr_HiveWrDMtWc_Wc = vei_CoR_veX(vei_HiveWrDMtWc, R=self.Wr(), C=self.Wc())
        DWrHiveWrDMtWcWc = self.D()[:,:,sp.newaxis] * Wr_HiveWrDMtWc_Wc
        RV = DMt - DWrHiveWrDMtWcWc
        RV = vei_CoR_veX(RV, R=self.Lr().T, C=self.Lc().T)
        if len(Mt.shape)==2:    RV = RV[:, :, 0]
        return RV
20238
def _K_computations(self, X, X2):
        """"""
        # First extract times and indices.
        self._extract_t_indices(X, X2)

        self._K_compute_eq()
        self._K_compute_ode_eq()
        if X2 is None:
            self._K_eq_ode = self._K_ode_eq.T
        else:
            self._K_compute_ode_eq(transpose=True)
        self._K_compute_ode()

        if X2 is None:
            self._K_dvar = np.zeros((self._t.shape[0], self._t.shape[0]))
        else:
            self._K_dvar = np.zeros((self._t.shape[0], self._t2.shape[0]))

        # Reorder values of blocks for placing back into _K_dvar.
        self._K_dvar = np.vstack((np.hstack((self._K_eq, self._K_eq_ode)),
                                                   np.hstack((self._K_ode_eq, self._K_ode))))
        self._K_dvar = self._K_dvar[self._rorder, :]
        self._K_dvar = self._K_dvar[:, self._rorder2]
        
        
        if X2 is None:
            # Matrix giving scales of each output
            self._scale = np.zeros((self._t.size, self._t.size))
            code="""
            for(int i=0;i<N; i++){
              scale_mat[i+i*N] = B[index[i]+output_dim*(index[i])];
              for(int j=0; j<i; j++){
                  scale_mat[j+i*N] = B[index[i]+output_dim*index[j]];
                  scale_mat[i+j*N] = scale_mat[j+i*N];
                }
              }
            """
            scale_mat, B, index = self._scale, self.B, self._index
            N, output_dim = self._t.size, self.output_dim
            weave.inline(code,['index',
                               'scale_mat', 'B',
                               'N', 'output_dim'])
        else:
            self._scale = np.zeros((self._t.size, self._t2.size))
            code = """
            for(int i=0; i<N; i++){
              for(int j=0; j<N2; j++){
                scale_mat[i+j*N] = B[index[i]+output_dim*index2[j]];
              }
            }
            """
            scale_mat, B, index, index2 = self._scale, self.B, self._index, self._index2
            N, N2, output_dim = self._t.size, self._t2.size, self.output_dim
            weave.inline(code, ['index', 'index2',
                                'scale_mat', 'B',
                                'N', 'N2', 'output_dim'])
1348818
def K(self, parm):
        
        return ARD_K_matrix(self.X, parm) + np.identity(self.X.shape[0])*(10**-10)
609305
def solve_kkt_ir(Q, D, G, A, rx, rs, rz, ry, niter=1):
    """"""
    nineq, nz, neq, nBatch = get_sizes(G, A)

    eps = 1e-7
    Q_tilde = Q + eps * torch.eye(nz).type_as(Q).repeat(nBatch, 1, 1)
    D_tilde = D + eps * torch.eye(nineq).type_as(Q).repeat(nBatch, 1, 1)

    dx, ds, dz, dy = factor_solve_kkt_reg(
        Q_tilde, D_tilde, G, A, rx, rs, rz, ry, eps)
    res = kkt_resid_reg(Q, D, G, A, eps,
                        dx, ds, dz, dy, rx, rs, rz, ry)
    resx, ress, resz, resy = res
    res = resx
    for k in range(niter):
        ddx, dds, ddz, ddy = factor_solve_kkt_reg(Q_tilde, D_tilde, G, A, -resx, -ress, -resz,
                                                  -resy if resy is not None else None,
                                                  eps)
        dx, ds, dz, dy = [v + dv if v is not None else None
                          for v, dv in zip((dx, ds, dz, dy), (ddx, dds, ddz, ddy))]
        res = kkt_resid_reg(Q, D, G, A, eps,
                            dx, ds, dz, dy, rx, rs, rz, ry)
        resx, ress, resz, resy = res
        # res = torch.cat(resx)
        res = resx

    return dx, ds, dz, dy