import numpy as np
from enum import Enum
from typing import List, Optional, Tuple
from dataclasses import dataclass
from scipy.linalg import null_space
import casadi as ca
from itertools import product
import logging
from mpt4py.base import Vector, Matrix, HData
from mpt4py.functions.affine_function import AffineFunction
from mpt4py.geometry import Polyhedron, PolyUnion
from mpt4py.tolerances import tolerances
from mpt4py.util import is_lexico_positive, lexico_argmin, lexico_argmax, Basis
from mpt4py.solvers.lcp_solver import lcp_solve, get_complement, get_variable_name, LcpSolverStatus
class PlcpSolverStatus(Enum):
PLCP_SOLVED = 0
PLCP_PRETERMINATED = -1
@dataclass
class PlcpSolution:
status: PlcpSolverStatus
regions: PolyUnion
bases: List[Basis]
logger = logging.getLogger(__name__)
[docs]
def plcp_solve(M: Matrix,
Q: Matrix,
q: Vector,
A_theta: Optional[Matrix] = None,
b_theta: Optional[Vector] = None,
solver: str = 'lemke',
zero_tol: float = tolerances['zero'], # zero tolerance
lex_tol: float = 1e-6, # lexicographic tolerance - a small treshold to determine if values are equal
max_pivots: int = 1000, # maximum number of pivots
verbose: bool = False, # verbose level
) -> PlcpSolution:
r"""
Solve a Parametric Linear Complementarity Problem (pLCP).
"""
if M.ndim != 2: raise ValueError("M must be a matrix.")
if q.ndim != 1: raise ValueError("q must be a vector.")
if M.shape[0] != M.shape[1]: raise ValueError("M must be a square matrix.")
if Q.ndim != 2: raise ValueError("Q must be a matrix.")
if q.size != M.shape[0]:
raise ValueError("Incompatible dimensions: number of rows and columns of M must match the number of elements in q.")
if np.linalg.matrix_rank(Q) != Q.shape[1]: raise ValueError("Q must be full column rank.")
d = Q.shape[1]
if A_theta is None and b_theta is None:
A_theta, b_theta = np.zeros((0, d)), np.zeros((0,))
elif A_theta is None and b_theta is not None or A_theta is not None and b_theta is None:
raise ValueError("A_theta and b_theta must be both None or non-None.")
else:
if A_theta.ndim != 2:
raise ValueError("A_theta must be a matrix.")
if b_theta.ndim != 1:
raise ValueError("b_theta must be a vector.")
if A_theta.shape[1] != d:
raise ValueError("Incompatible dimensions: number of columns of A_theta must match the number of columns of Q.")
if A_theta.shape[0] != b_theta.size:
raise ValueError("Incompatible dimensions: number of rows of A_theta must match the number of elements in b_theta.")
if Polyhedron(HData(A_theta, b_theta)).is_empty:
raise ValueError("A_theta and b_theta results in empty parameter feasible region.")
logging.basicConfig(filename='plcp.log', level=logging.INFO)
logger.info('Start to solve Parametric Linear Complementarity Problem.\n')
logger.info("Matrix M:\n%s", M)
logger.info("Vector q:\n%s", q)
logger.info("Matrix Q:\n%s", Q)
if solver in ['lemke', 'graves']:
critical_regions, bases = _geometrical_approach(M, Q, q, A_theta, b_theta, method=solver, zero_tol=zero_tol, lex_tol=lex_tol, max_pivots=max_pivots, verbose=verbose)
return PlcpSolution(status=PlcpSolverStatus.PLCP_SOLVED, regions=critical_regions, bases=bases)
elif solver == 'exhaustive':
critical_regions, bases = _exhaustive_enumeration(M, Q, q, A_theta, b_theta, zero_tol=zero_tol, verbose=verbose)
return PlcpSolution(status=PlcpSolverStatus.PLCP_SOLVED, regions=critical_regions, bases=bases)
else:
raise ValueError("Unsupported solver.")
def _exhaustive_enumeration(M: Matrix, Q: Matrix, q: Vector, A_theta: Matrix, b_theta: Vector,
zero_tol: float = tolerances['zero'], # zero tolerance
verbose: bool = False) -> Tuple[PolyUnion, List[Basis]]:
n, d = Q.shape
param_space = Polyhedron.from_Hrep(A_theta, b_theta)
A = np.hstack([np.eye(n), -M])
critical_regions_list = []
bases_list = []
# Enumerate all possible bases
for selector in product([0, 1], repeat=n):
indices = [i if s == 0 else i + n for i, s in enumerate(selector)]
basis = Basis(indices, n)
if np.abs(np.linalg.det(A[:, basis.indices])) <= zero_tol:
continue
Ainv_b = np.linalg.solve(A[:, basis.indices], np.hstack([-Q, q[:, np.newaxis]]))
# If x_i = beta_i (Q*theta+q) = 0 for all theta, it means this basis can define the same critical region with another basis
zero_rows = np.where(np.linalg.norm(Ainv_b, axis=1) < zero_tol)[0]
if len(zero_rows) > 0 and np.any(np.array(basis.indices, dtype=int)[zero_rows] > n-1):
continue
critical_region = Polyhedron.from_Hrep(Ainv_b[:, :-1], Ainv_b[:, -1]).intersect(param_space)
if critical_region.is_full_dim:
critical_region.minHrep()
Kwz = np.zeros((2*n, d))
Kwz[basis.indices, :] = -Ainv_b[:, :-1]
kwz = np.zeros((2*n,))
kwz[basis.indices] = Ainv_b[:, -1]
critical_region.add_function(func=AffineFunction(Kwz[:n, :], kwz[:n]), func_name='w') # w = Kw*theta + kw
critical_region.add_function(func=AffineFunction(Kwz[n:, :], kwz[n:]), func_name='z') # z = Kz*theta + kz
critical_regions_list.append(critical_region)
bases_list.append(basis)
else:
continue
return PolyUnion(*critical_regions_list), bases_list
def _geometrical_approach(M: Matrix, Q: Matrix, q: Vector, A_theta: Matrix, b_theta: Vector,
method: str = 'lemke',
zero_tol: float = tolerances['zero'], # zero tolerance
lex_tol: float = 1e-6, # lexicographic tolerance to determine if values are equal
max_pivots: int = 1000, # maximum number of pivots
verbose: bool = False, # verbose level
) -> Tuple[PolyUnion, List[Basis]]:
n = Q.shape[0]
d = Q.shape[1]
A = np.hstack([np.eye(n), -M])
param_space = Polyhedron(HData(A_theta, b_theta)) if np.size(A_theta) else None
unexplored_bases = set()
discovered_bases = set()
discovered_critical_regions = []
discovered_bases_list = []
# Initialisation, find a feasible complementary basis B0
theta_init = _compute_initial_feasible_param(M, Q, q, A_theta, b_theta)
lcp_solution = lcp_solve(M, q + Q@theta_init)
if lcp_solution.status != LcpSolverStatus.LCP_FEASIBLE:
raise RuntimeError("Failed to solve LCP with initial parameter theta. LCP solver returns status {0}.".format(lcp_solution.status))
B0 = Basis(lcp_solution.basis, n)
if verbose:
print("Successfully computed initial feasible parameter.")
print("Initial feasible parameter is: theta = {}".format(theta_init))
print("Initial complementary basis is: {}".format(lcp_solution.basis))
discovered_bases.add(B0)
unexplored_bases.add(B0)
itr = 0
# TODO: count the number of pivots
while len(unexplored_bases) and itr < max_pivots:
if verbose:
print("\n=================== Iteration", itr, "===================")
print("\nNumber of unexplored_basis: ", len(unexplored_bases))
for unexplored_basis in unexplored_bases:
print(unexplored_basis)
itr += 1
# select any basis from unexplored_bases and remove it
basis_to_explore = unexplored_bases.pop()
# compute the critical w.r.t to the current basis, explore this basis
# beta = np.linalg.solve(A[:, basis_to_explore.indices], np.eye(n))
# assert np.abs(np.linalg.det(A[:, basis_to_explore.indices])) > zero_tol, "The given basis indices leads to a singular A_B matrix."
if not np.abs(np.linalg.det(A[:, basis_to_explore.indices])) > zero_tol:
continue # skip singular basis
beta_Qq = np.linalg.solve(A[:, basis_to_explore.indices], np.hstack([Q, q.reshape(-1,1)]))
beta_Q = beta_Qq[:, :-1]
beta_q = beta_Qq[:, -1]
region_to_explore = Polyhedron.from_Hrep(-beta_Q, beta_q)
if not region_to_explore.is_full_dim:
Warning("The region to explore is empty. Meaning the pivoting function is resulting in a non-full-dimension CR.")
continue
region_intersect_parameter_set = region_to_explore if param_space is None else region_to_explore.intersect(param_space)
# ! Should change to not is_full_dim?
if not region_intersect_parameter_set.is_full_dim:
Warning("The region to explore is non-full-dimension CR.")
continue
region_intersect_parameter_set.minHrep(use_cdd=True)
Kwz = np.zeros((2 * n, d))
Kwz[basis_to_explore.indices, :] = beta_Q
kwz = np.zeros((2 * n,))
kwz[basis_to_explore.indices] = beta_q
region_intersect_parameter_set.add_function(AffineFunction(Kwz[:n, :], kwz[:n]), 'w')
region_intersect_parameter_set.add_function(AffineFunction(Kwz[n:, :], kwz[n:]), 'z')
discovered_bases_list.append(basis_to_explore)
discovered_critical_regions.append(region_intersect_parameter_set)
# ! Calling minVrep is not necessary. However, if I do not call it, the results will be wrong. The H-rep LHS is normailized somewhere but the RHS is not!
# ! I don't know why yet.
region_intersect_parameter_set.minVrep()
if verbose:
print("\nRegion to explore:")
print("H-rep:")
print(region_intersect_parameter_set.H)
print("V-rep:")
print(region_intersect_parameter_set.V)
# for each facet of the closure of this region, find its neighbors by pivoting
number_of_facets = np.size(region_intersect_parameter_set.b)
for facet_index in range(number_of_facets):
if verbose:
print("\n------------------ Itr {}, facet {} ------------------".format(itr, facet_index))
print("Current basis is: ", basis_to_explore.indices)
print(region_intersect_parameter_set.H[facet_index, :])
if method == 'lemke':
adjacent_region_bases = _find_adj_critical_regions_lemke(M, Q, q, basis_to_explore, region_intersect_parameter_set, facet_index, verbose)
elif method == 'graves':
adjacent_region_bases = _find_adj_critical_regions_graves(M, Q, q, basis_to_explore, region_intersect_parameter_set, facet_index, zero_tol=zero_tol, verbose=verbose)
else:
raise ValueError("Not supported geometrical approach.")
if verbose:
print('Computed adj basis is: ', )
list_adj = list(adjacent_region_bases)
for i in range(len(list_adj)):
print(list_adj[i].indices)
print(get_variable_name(list_adj[i].indices, n))
# line 6 and 7 in Algorithm 1 in CNJ2006
unexplored_bases = unexplored_bases.union(adjacent_region_bases - discovered_bases)
discovered_bases = discovered_bases.union(adjacent_region_bases)
print("\n-------------------------------------------------")
print('Multi-parametric linear complementarity problem')
print("{:<25} {:<10} ".format('Num variables:', n))
print("{:<25} {:<10} ".format('Num parameters:', d))
print("{:<25} {:<10} ".format('Num critical regions:', len(discovered_critical_regions)))
print("{:<25} {:<10} ".format('Num bases:', len(discovered_bases)))
print("-------------------------------------------------")
list_discoverd_bases = list(discovered_bases)
if verbose:
for i in range(len(discovered_bases)):
print(sorted(list_discoverd_bases[i].indices))
return PolyUnion(*discovered_critical_regions), discovered_bases_list
def _compute_initial_feasible_param(M: Matrix, Q: Matrix, q: Vector, A_theta: Matrix, b_theta: Vector) -> Vector:
"""
Find a parameter theta s.t the LCP (q+Q*theta, M) is feasible.
Relax the complementarity constraints w^T z = 0 and solve the following problem:
min 1^T * [\theta; x]
s.t. A * x = q + Q * theta,
x >= 0,
A_theta * theta <= b_theta
where x = [w; z], A = [I, -M].
Q has a shape of (n, d)
Returns:
np.ndarray with a shape of (d,)
"""
n = Q.shape[0]
d = Q.shape[1]
opti = ca.Opti()
x = opti.variable(2*n)
theta = opti.variable(d)
opti.subject_to(x >= 0)
if np.size(A_theta):
opti.subject_to(A_theta @ theta <= b_theta)
opti.subject_to(np.hstack((np.eye(n), -M)) @ x == q + Q @ theta)
opti.minimize(ca.dot(np.ones(2*n), x) + ca.dot(np.ones(d), theta))
options = {'ipopt.print_level':0, 'print_time':0, 'ipopt.tol':1e-8}
opti.solver('ipopt', options)
sol = opti.solve()
if not sol.stats()['success']:
raise RuntimeError("Ipopt failure.")
theta_opt = sol.value(theta)
return np.reshape(theta_opt, -1)
def _find_adj_critical_regions_graves(M: Matrix, Q: Matrix, q: Vector,
basis: Basis, critical_region: Polyhedron, facet_index: int, zero_tol: float = tolerances['zero'], verbose: bool = False) -> set[Basis]:
# we follow the notations in Section III.B in CNJ2006
# get normalized facet equation gamma'*theta = b, where ||gamma||_2 = 1
gamma = critical_region.A[facet_index, :]
b = critical_region.b[facet_index]
gamma_norm = np.linalg.norm(gamma)
gamma /= gamma_norm
b /= gamma_norm
return _pivot_graves(M, Q, q, basis, critical_region, gamma, b, zero_tol=zero_tol, verbose=verbose)
def _pivot_graves(M: Matrix, Q: Matrix, q: Vector, basis: Basis, critical_region: Polyhedron, gamma: Vector, b: float, zero_tol: float = tolerances['zero'], verbose: bool = False) -> set[Basis]:
n, d = Q.shape
A = np.hstack([np.eye(n), -M])
unexplored_bases = {Basis(basis.indices, n)}
while len(unexplored_bases):
current_basis = unexplored_bases.pop()
assert np.abs(np.linalg.det(A[:, current_basis.indices])) > zero_tol, "The given basis indices leads to a singular A_B matrix."
beta, _, _, _ = np.linalg.lstsq(A[:, current_basis.indices], np.eye(n), rcond=None)
assert np.allclose(beta @ A[:, current_basis.indices], np.eye(n)), "A_B is not invertible."
if d == 1:
raise NotImplementedError
else:
N = null_space(gamma.reshape((1,-1)))
# find the set Z {i | beta_i [Q*N, q+Q*gamma*b] = 0}, in which the leaving variable does not depend on theta_f
test_matrix_Z = beta @ np.column_stack([Q@N, q+Q@gamma*b])
is_zero = np.where(np.linalg.norm(test_matrix_Z, axis=1) < np.sqrt(d)*zero_tol)[0].tolist()
is_neg = np.where(beta @ Q @ gamma < -zero_tol)[0].tolist()
if len(is_zero) == 0:
if verbose: print("{:<25} {:<10}".format('Pivoting status:', 'On the boundary facet'))
return set()
if len(is_neg) == 0:
if verbose: print("{:<25} {:<10}".format('Pivoting status:', 'Feasible solution unbounded'))
return set()
is_neg_zero = list(set(is_zero).intersection(set(is_neg)))
assert len(is_neg_zero) > 0, "Interesting... is_neg_zero is empty"
r_test_matrix = beta[is_neg_zero, :] / (beta@Q@gamma)[is_neg_zero, np.newaxis]
r = is_neg_zero[lexico_argmax(r_test_matrix)[0]] # r is the row index, not index of variables
# fbar = r_test_matrix[r, :]
y_r = current_basis.indices[r] # the index of variable for r row
t_r = (y_r + n) % (2*n) # choose the entering variable
A_bar_r = beta @ A[:, t_r]
# * case 1, single pivot
if abs(A_bar_r[r]) > zero_tol:
if verbose:
print("{:<25} {:<10}".format('Pivoting status:', 'Case 1, a_r is nonzero (single pivot)'))
current_basis.indices[r] = t_r
# Termination condition
if (np.linalg.inv(A[:, current_basis.indices]) @ Q @ gamma)[r] > zero_tol:
return {current_basis}
# * case 2,
elif np.all(A_bar_r < zero_tol): # all non-positive
if verbose:
print("{:<25} {:<10}".format('Pivoting status:', 'Case 2, all a_ir are non-positive (infeasible)'))
return set() # infeasible problem, gone over the edge
# * case 3,
else:
if verbose:
print("{:<25} {:<10}".format('Pivoting status:', 'Case 3, exist positive a_ir (double pivot)'))
ispos = np.where(A_bar_r > zero_tol)[0]
discovered_bases = set()
# for i in range(n):
for i in np.where(A_bar_r > zero_tol)[0]:
if i == r:
continue
# if A_bar_r[i] < zero_tol:
# continue
new_indices = current_basis.indices.copy()
new_indices[r] = get_complement(new_indices[r], n)
new_indices[i] = get_complement(new_indices[i], n)
if np.abs(np.linalg.det(A[:, new_indices])) < zero_tol:
continue # if A_B is singular, ignore it
beta_times_Qq = np.linalg.solve(A[:, new_indices], np.hstack([Q, q[:,np.newaxis]]))
new_critical_region = Polyhedron.from_Hrep(A = -beta_times_Qq[:, :-1], b = beta_times_Qq[:, -1])
# if the new basis indices lead to a full-dimension CR, it means we found an adjacent CR, add it into the found set
if new_critical_region.affine_dimension == d:
discovered_bases.add(Basis(new_indices, n))
# if the new basis indices lead to a CR of dim d-1, we need to check if it intersects with the current critical region facet
elif new_critical_region.affine_dimension == d-1:
if new_critical_region.intersect(critical_region).affine_dimension == d-1:
new_discovered = _pivot_graves(M, Q, q, Basis(new_indices, n), critical_region, gamma, b, zero_tol=zero_tol, verbose=verbose)
discovered_bases = discovered_bases.union(new_discovered)
else:
continue
else:
continue
return discovered_bases
def _find_adj_critical_regions_lemke(M: Matrix, Q: Matrix, q: Vector,
basis: Basis, critical_region: Polyhedron, facet_index: int, verbose: bool = False) -> set[Basis]:
"""
Compute the bases which define the adjacent critical regions along the given facet of current critical region.
Each facet can have multiple adjacent critical regions in non-facet-to-facet case
Args:
basis: the basis which defines the current critical region
region: current critical region of which the adjacent regions will be computed
facet_index: index of the facet to step over
Returns:
A set of Basis which defines the adjacent critical regions given a facet
"""
n = Q.shape[0]
d = Q.shape[1]
# we follow the notations in Section III.B in CNJ2006
# get normalized facet equation gamma'*theta + b = 0, where ||gamma||_2 = 1
gamma = critical_region.A[facet_index, :]
b = -critical_region.b[facet_index]
gamma_norm = np.linalg.norm(gamma)
gamma /= gamma_norm
b /= gamma_norm
if d == 1:
# For parameter space of 1-dim, reduce to non-parametric LCP
Q_hat = np.zeros((n, d)) # TODO: check this
N = np.zeros((d, d-1)) # TODO: check this
else:
N = null_space(gamma.reshape((1,-1)))
Q_hat = Q @ N
q_hat = q - Q @ gamma * b
gamma_hat = -Q @ gamma
# remove the current hyperplane gamma'theta+b=0 from the critical region first
# otherwise might get an empty F_hat due to numerical issue
A_other_rows = np.delete(critical_region.A, facet_index, axis=0)
b_other_rows = np.delete(critical_region.b, facet_index)
F_hat = Polyhedron(H=HData(A_other_rows @ N, A_other_rows @ gamma * b + b_other_rows))
# initialization step, according to Section III.B.1.d: Initialisation
# bring alpha into feasible complementary basis B by calling the function pivot()
# with alpha (corresponds to index 2n) as the entering variable
adjacent_basis_set = _pivot_lemke(M, Q, q, basis, 2*n, Q_hat, q_hat, gamma_hat, F_hat, verbose=verbose)
return adjacent_basis_set
def _pivot_lemke(M: Matrix, Q: Matrix, q: Vector, aclfb: Basis, entering_var_index: int,
Q_hat: Matrix, q_hat: Matrix, gamma_hat: Matrix, F_hat: Polyhedron, zero_tol: float = tolerances['zero'], verbose: bool = False) -> set[Basis]:
"""
The pivot function in Algorithm 2 in CNJ2006.
The goal of this function is to find the basis of the adjacent critical region.
Args:
aclfb: an almost complementary lexico-feasible basis (ACFLB), defined in Def.4 in CNJ2006.
entering_var_index: the index of entering variable.
Returns:
the bases of adjacent critical regions.
"""
n = Q.shape[0]
d = Q.shape[1]
if verbose:
print('\n')
print("{:<25} {:<10}".format('Entering variable:', get_variable_name(entering_var_index, n)))
A_hat = np.column_stack([np.eye(n), -M, gamma_hat])
try:
beta, _, _, _ = np.linalg.lstsq(A_hat[:, aclfb.indices], np.eye(n), rcond=None)
except np.linalg.LinAlgError:
raise np.linalg.LinAlgError("The least square problem didn't converge")
# find the set: P = {i | beta_{B(i),*} * A_hat_{*,e} > 0}
test_vector_P = beta @ A_hat[:, entering_var_index]
list_P = np.where(test_vector_P > zero_tol)[0].tolist()
if len(list_P) == 0:
if verbose:
# print("{:<25} {:<10}".format('Leaving variable:', 'alpha'))
print("{:<25} {:<10}".format('Pivoting status:', 'Feasible solution unbounded'))
return set()
# find the set: Z = {i | beta_{B(i),*} [Q_hat, q_hat] = 0}
test_matrix_Z = beta @ np.column_stack([Q_hat, q_hat])
list_Z = np.where(np.linalg.norm(test_matrix_Z, axis=1) < np.sqrt(d)*zero_tol)[0].tolist()
if len(list_Z) == 0:
if verbose:
# print("{:<25} {:<10}".format('Leaving variable:', 'alpha'))
print("{:<25} {:<10}".format('Pivoting status:', 'On the boundary facet'))
return set()
# raise RuntimeError("The set Z should never be empty exact on the boundary facet!")
list_intersection_P_Z = list(set(list_P).intersection(set(list_Z)))
# * Termination condition (Algorithm 2, line 5,6 in CNJ2006, Theorem 5)
# line 5, 1st and 2nd condition
if len(list_intersection_P_Z) == 0 and (2*n in aclfb.indices):
# find the index of alpha in basis
alpha_idx = aclfb.indices.index(n*2)
# line 5, 3rd condition
if beta[alpha_idx,:] @ A_hat[:, entering_var_index] < -zero_tol:
new_aclfb = Basis(aclfb.indices, n)
new_aclfb.indices.append(entering_var_index)
new_aclfb.indices.remove(2*n)
new_aclfb_set = set()
new_aclfb_set.add(new_aclfb)
if verbose:
print("{:<25} {:<10}".format('Leaving variable:', 'alpha'))
print("{:<25} {:<10}".format('Pivoting status:', 'Termination condition reached'))
print("Terminate with new basis: ", new_aclfb.indices)
return new_aclfb_set
# * Case 1, Z intersect P is non-empty (Algorithm 2, line 8-10)
if len(list_intersection_P_Z):
rows_to_judge = beta[list_intersection_P_Z, :] / np.dot(beta[list_intersection_P_Z, :], A_hat[:, entering_var_index])[:, np.newaxis]
# TODO: consider change this to lex_tol
l = lexico_argmin(rows_to_judge, zero_tol)
if len(l) != 1:
raise ValueError("Exist more than one lexico minimum")
pivot_row_index = list_intersection_P_Z[l[0]]
# need to transform pivot_row_index to leaving_var_index
leaving_var_index = aclfb.indices[pivot_row_index]
if verbose:
print("{:<25} {:<10}".format('Leaving variable:', get_variable_name(leaving_var_index, n)))
print("{:<25} {:<10}".format('Pivoting status:', 'Case 1 (facet_to_facet)'))
new_aclfb = Basis(aclfb.indices, n)
new_aclfb.indices.append(entering_var_index)
new_aclfb.indices.remove(leaving_var_index)
new_entering_var_index = (leaving_var_index+n) % (2*n)
return _pivot_lemke(M, Q, q, new_aclfb, new_entering_var_index, Q_hat, q_hat, gamma_hat, F_hat, zero_tol=zero_tol, verbose=verbose)
# * Case 2, Z intersect P is empty (Algorithm 2, line 11-19)
else:
if verbose:
# print("{:<25} {:<10} ".format('Leaving variable:', get_variable_name(leaving_var_index, n)))
print("{:<25} {:<10}".format('Pivoting status:', 'Case 2 (non facet_to_facet)'))
set_L = set()
for l in list_P: # Algorithm 2, line 13
if verbose:
print("i = {}".format(l))
# test if conditions in Theorem 4 is satisfied
Gamma_i = beta[list_P,:] @ A_hat[:,[entering_var_index]] @ beta[[l],:] - \
(beta[l,:] @ A_hat[:,entering_var_index]) * beta[list_P, :]
# TODO: check this one! I suspect the formula on paper is wrong
Gamma_i = -Gamma_i
# find those rows satisfying: Gamma_i Q_hat = 0 in Theorem 4
iszero = np.where(np.linalg.norm(Gamma_i @ Q_hat, axis=1) < np.sqrt(Q_hat.shape[1]) * zero_tol)[0].tolist()
if len(iszero) == 0:
continue
# check if Gamma_i [q_hat I] is lexico positive
is_lex_pos = is_lexico_positive(Gamma_i[iszero, :] @ np.column_stack((q_hat, np.eye(n))), zero_tol)
if not is_lex_pos:
continue
first_set = Polyhedron(HData(-Gamma_i@Q_hat, Gamma_i@q_hat))
if first_set.is_empty:
# print("First set is empty")
continue
cr = first_set.intersect(F_hat)
if not cr.is_full_dim or cr.is_empty or not cr.is_bounded:
# print("Not full dim")
continue
new_aclfb = Basis(aclfb.indices, n)
leaving_var_index = aclfb.indices[l]
new_aclfb.indices.remove(leaving_var_index)
new_aclfb.indices.append(entering_var_index)
pivot_row_index = l
leaving_var_index = aclfb.indices[pivot_row_index]
new_entering_var_index = (leaving_var_index+n) % (2*n)
if verbose:
print("{:<25} {:<10} ".format('Leaving variable:', get_variable_name(leaving_var_index, n)))
F_prime = F_hat.intersect(first_set)
set_L = set_L.union(_pivot_lemke(M, Q, q, new_aclfb, new_entering_var_index, Q_hat, q_hat, gamma_hat, F_prime, zero_tol=zero_tol, verbose=verbose))
if verbose:
print(". Set L is: ", set_L)
# ! might need to change this part for verbosing the founded bases
if verbose:
print('Found adjacent basis: ', set_L)
return set_L