void construct_lcp_PHI(cache_config& config)
{
static_assert(t_width == 0 or t_width == 8 , "construct_lcp_PHI: width must be `0` for integer alphabet and `8` for byte alphabet");
typedef int_vector<>::size_type size_type;
typedef int_vector<t_width> text_type;
const char* KEY_TEXT = key_text_trait<t_width>::KEY_TEXT;
int_vector_buffer<> sa_buf(config.file_map[conf::KEY_SA]);
size_type n = sa_buf.size();
assert(n > 0);
if (1 == n) { // Handle special case: Input only the sentinel character.
int_vector<> lcp(1, 0);
store_to_cache(lcp, conf::KEY_LCP, config);
return;
}
// (1) Calculate PHI (stored in array plcp)
int_vector<> plcp(n, 0, sa_buf.width());
for (size_type i=0, sai_1 = 0; i < n; ++i) {
size_type sai = sa_buf[i];
plcp[ sai ] = sai_1;
sai_1 = sai;
}
// (2) Load text from disk
text_type text;
load_from_cache(text, KEY_TEXT, config);
// (3) Calculate permuted LCP array (text order), called PLCP
size_type max_l = 0;
for (size_type i=0, l=0; i < n-1; ++i) {
size_type phii = plcp[i];
while (text[i+l] == text[phii+l]) {
++l;
}
plcp[i] = l;
if (l) {
max_l = std::max(max_l, l);
--l;
}
}
util::clear(text);
uint8_t lcp_width = bits::hi(max_l)+1;
// (4) Transform PLCP into LCP
std::string lcp_file = cache_file_name(conf::KEY_LCP, config);
size_type buffer_size = 1000000; // buffer_size is a multiple of 8!
int_vector_buffer<> lcp_buf(lcp_file, std::ios::out, buffer_size, lcp_width); // open buffer for lcp
lcp_buf[0] = 0;
sa_buf.buffersize(buffer_size);
for (size_type i=1; i < n; ++i) {
size_type sai = sa_buf[i];
lcp_buf[i] = plcp[sai];
}
lcp_buf.close();
register_cache_file(conf::KEY_LCP, config);
}
void LCPSelfTest() {
Matrix A(3,3);
Vector b(3);
{
A(0,0) = 0; A(0,1) = 0; A(0,2) = 1;
A(1,0) = 0; A(1,1) = 0; A(1,2) = 1;
A(2,0) = -1; A(2,1) = -1; A(2,2) = 0;
b(0) = -2;
b(1) = -1;
b(2) = 3;
LemkeLCP lcp(A,b);
lcp.verbose = 3;
if(!lcp.Solve()) {
cerr<<"LCP didn't solve!"<<endl;
Abort();
}
Vector w,z;
lcp.GetW(w);
lcp.GetZ(z);
cout<<"W: "<<VectorPrinter(w)<<endl;
cout<<"Z: "<<VectorPrinter(z)<<endl;
}
{
A(0,0) = -1; A(0,1) = -1; A(0,2) = 0;
A(1,0) = 0; A(1,1) = 0; A(1,2) = 1;
A(2,0) = 0; A(2,1) = 0; A(2,2) = 1;
b(0) = 2;
b(1) = -1;
b(2) = -1;
LemkeLCP lcp(A,b);
lcp.verbose = 3;
if(!lcp.Solve()) {
cerr<<"LCP didn't solve!"<<endl;
Abort();
}
Vector w,z;
lcp.GetW(w);
lcp.GetZ(z);
cout<<"W: "<<VectorPrinter(w)<<endl;
cout<<"Z: "<<VectorPrinter(z)<<endl;
}
}
/*
printa(int a[],int n) {
for(int i=0;i<=n;i++) printf("%d ",a[i]);
printf("\n");
}
*/
int main() {
while(scanf("%s",str)==1) {
int len=strlen(str);
int n=2*len+1;
for(int i=0;i<len;i++) r[i]=str[i];
for(int i=0;i<len;i++) r[len+i+1]=str[len-1-i];
r[n]=0;
r[len]=1;
da(r,sa,rank,height,n,128);
for(int i=1;i<=n;i++) RMQ[i]=height[i];
initRMQ(n);
int ans=0,st,tmp;
for(int i=0;i<len;i++) {
tmp=lcp(i,n-i);
if(2*tmp>ans) {
ans=2*tmp;
st=i-tmp;
}
tmp=lcp(i,n-i-1);
if(2*tmp-1>ans) {
ans=2*tmp-1;
st=i-tmp+1;
}
//printf("%d %d %d\n",ans,tmp,st);
}
str[st+ans]=0;
printf("%s\n",str+st);
/*
printa(sa,len);
printa(rank,len);
printa(height,len);
for(int i=0;i<len-1;i++) {
for(int j=i+1;j<len;j++) {
printf("%d-%d:lcp=%d\n",i,j,lcp(i,j));
}
}
*/
}
return 0;
}
string longestCommonPrefix(vector<string>& strs) {
string r;
int n = strs.size();
if(n==0) return r;
r = strs[0];
for(int i=1; i<n; i++)
{
r = lcp(r, strs[i]);
if(r.size() == 0)
break;
}
return r;
}
SfaType GSA::getRightBoundOnGSA(const SfaChar* pat, int len, SfaType l, SfaType r)
{
if ( l > r ) return r;
if ( verbose ) std::cout << "Right bound:\n";
int lLcp = lcp((SfaChar*)getSeq(0), pat);
int rLcp = lcp((SfaChar*)getSeq(size-1), pat);
if ( verbose ) printf("lLcp:%d\trLcp:%d\n", lLcp, rLcp);
if (lLcp < len && pat[lLcp] <= getSeq(0)[lLcp]) return 0;
if (rLcp >= len || pat[rLcp] >= getSeq(size-1)[rLcp]) return size;
// SfaType left = 1;
// SfaType right = size;
size_t left = l;
size_t right = r;
while (right-left > 1)
{
//size_t mid = left + floor((right-left)/2.0);
size_t mid = (right+left)/2.0;
int mLcp = lLcp <= rLcp? lLcp : rLcp;
mLcp += lcp((SfaChar*)getSeq(mid-1)+mLcp, pat+mLcp);
if ( verbose )
printf("left:%zu right:%zu mid:%zu lLcp:%d rLcp:%d\n", left, right, mid, lLcp, rLcp);
if (mLcp >= len || pat[mLcp] >= getSeq(mid-1)[mLcp]) {
left = mid; lLcp = mLcp;
}
else {
right = mid; rLcp = mLcp;
}
if ( verbose )
printf("left:%zu right:%zu mid:%zu lLcp:%d rLcp:%d mLcp:%d\n", left, right, mid, lLcp, rLcp, mLcp);
}
if ( verbose ) std::cout << "right bound:" << left << "\n";
return left;
}
int main(int argc, char** argv){
int i;
if( argc < 2 ) exit(1);
char* str = argv[1];
int lcp_array[strlen(str)];
lcp(lcp_array, str, strlen(str));
i = 0;while( i < strlen(str)){
printf("%d ", lcp_array[i]);
i++;
}
printf("\n");
return 0;
}
// O(n).
// lcp[i] means lcp(str[sa[i]..], str[sa[i + 1]..]).
std::vector<int> get_lcp() {
for (int i = 0; i <= n; i++)
rank[sa[i]] = i;
int h = 0;
std::vector<int> lcp(n + 1);
for (int i = 0; i < n; i++) {
int j = sa[rank[i] - 1];
if (h > 0)
h--;
for (; j + h < n && i + h < n; h++) {
if (str[j + h] != str[i + h])
break;
}
lcp[rank[i] - 1] = h;
}
return lcp;
}
std::vector<int> longest_common_prefix(const T &s, const SuffixArray &sa){
const int n = sa.size();
std::vector<int> vs(n), isa(n), lcp(n - 1);
for(int i = 0; i + 1 < n; ++i){ vs[i] = s[i]; }
for(int i = 0; i < n; ++i){ isa[sa[i]] = i; }
int h = 0;
for(int i = 0; i < n; ++i){
const int j = isa[i];
if(j > 0){
const int k = j - 1;
while(vs[sa[j] + h] == vs[sa[k] + h]){ ++h; }
lcp[k] = h;
if(h > 0){ --h; }
}
}
return lcp;
}
vector<int> buildLcp(const vector<int> &str, const vector<int> &sa) {
// lcp: longest common prefix for sa[i-1] sa[i]
int len = sa.size();
vector<int> lcp(len, 0), idx(len);
for(int i=0; i<len; ++i) idx[sa[i]] = i;
for(int i=0, l=0; i<len; ++i) {
if( idx[i] == 0 ) {
l = 0;
continue;
}
int j = sa[idx[i]-1];
while( i+l<len && j+l<len && str[i+l]==str[j+l] )
++l;
lcp[idx[i]] = l;
l -= l>0;
}
return lcp;
}
int main()
{
int t,length,sum;
while(EOF!=scanf("%d",&t))
{
if(t==0)
break;
memset(str,0,sizeof(str));
getchar();
gets(str);
len1=strlen(str);
length=len1;
str[len1++]=2;
gets(str+len1);
len2=strlen(str+len1);
n=len1+len2;
str[n++]=1;
suffixArray();
lcp();
init();
for(i=2;i<n;i++)
{
stack[i-1].index=SA[i]+1;
if(stack[i-1].index>=9)
stack[i-1].flag=true;
else
stack[i-1].flag=false;
}
length=2*length;sum=0;
for(i=1;i<length;i++)
{
j=i+1;
while(RMQ(stack[i].index,stack[j].index)>=t)
{
if(stack[i].flag!=stack[j].flag)
sum+=RMQ(stack[i].index,stack[j].index)-t+1;
j++;
}
}
printf("%d\n",sum);
}
return 0;
}
string longestCommonPrefix(vector<string> &strs) {
if(strs.size() == 0)
return "";
vector<string> res;
while(strs.size() > 1)
{
for(int i=0; i<strs.size(); i+=2)
{
if(i+1 < strs.size())
res.push_back(lcp(strs[i], strs[i+1]));
else
res.push_back(strs[i]);
}
strs = res;
res.clear();
}
return strs[0];
}
int main(void)
{
out("starting ...\n");
std::cout << " Three way quick sort " << std::endl;
std::vector<char*> strings;
int N = 0;
//start from
char str[256] = {0};
while( scanf("%s",&str) != EOF ) {
char * copy = strdup(str);
strings.push_back(copy);
int len = strnlen(copy, 256);
for(int i = 1; i < len-1; i++) {
strings.push_back(©[i]);
}
}
N = strings.size();
std::vector<int> v(N); //map to sorted strings
for(int i = 0; i < N; i++)
v[i] = i; //all v's entries point to initial respective locations
threeway_quicksort(strings, v, 0, N, 0);
out("SORTED strings\n");
for(int i = 0; i < N; i++) {
out ("%s\n", strings[v[i]]);
}
int f = binary_search(strings, v, str, 0, N);
if(f > 0)
printf("found %s\n", strings[v[f]]);
else
printf("Not found %s\n", str);
lcp(strings, v);
return 0;
}
static void test_lemke()
{
real * A = makeRandMatrix();
real * b = makeRandVec2();
real * x = (real *)malloc(2*NN*sizeof(real));
real * xx = (real *)malloc(2*NN*sizeof(real));
real * AA = makeMatrix();
real * bb = makeRandVec2();
std::vector<real *> vx;
// copyMatrix(A,exaples_m2);
// for(int i =0;i<N;i++)
// b[i] = exaples_q2[i];
copyMatrix(AA,A);
memcpy(bb,b,NN*sizeof(real));
//memset(x,0,sizeof(real)*N);
memset(x,0,NN*sizeof(real));
//memcpy(xx,b,N*sizeof(real));
memset(xx,0,sizeof(real)*NN);
printf("--------------------------------------------------------\n");
printMat("solve lcp A=",A);
printVec("lcp b=",b);
printf("--------------------------------------------------------\n");
int result1 = lcp(A,b,vx,NN);
int result2 = lcp_lemke(A,b,x,NN);
printf("lcp solve:\n");
printf("--------------------------------------------------------\n");
printLCPVx(vx);
printf("lcp_lemke solve %s (%d)\n",result2?"true":"false",result2);
printVec("lcp_lemke=",x);
check_lcp_result(AA,bb,x,NN);
freeMatrix(A);
freeMatrix(b);
freeMatrix(x);
freeMatrix(AA);
freeMatrix(bb);
freeLcpSolve(vx);
}
static void test_pgs()
{
real * A = makeRandSPDMatrix();
real * b = makeRandVec2();
real * x = makeRandVec2();
real * xx = makeRandVec2();
real * AA = makeMatrix();
real * bb = makeRandVec2();
std::vector<real *> vx;
copyMatrix(AA,A);
memcpy(bb,b,NN*sizeof(real));
//memset(x,0,sizeof(real)*N);
memset(x,0,NN*sizeof(real));
//memcpy(xx,b,N*sizeof(real));
memset(xx,0,sizeof(real)*NN);
printf("--------------------------------------------------------\n");
printMat("solve lcp A=",A);
printVec("lcp b=",b);
printf("--------------------------------------------------------\n");
int result1 = lcp(A,b,vx,NN);
int result2 = lcp_pgs(A,b,x,NN,15,0.001);
int result3 = Solve_GaussSeidel(AA,bb,xx,NN,15);
printf("lcp solve:\n");
printf("--------------------------------------------------------\n");
printLCPVx(vx);
printf("lcp_pgs solve %s (%d)\n",result2?"true":"false",result2);
printVec("lcp_pgs=",x);
check_lcp_result(AA,bb,x,NN);
printVec("gs solve :",xx);
freeMatrix(A);
freeMatrix(b);
freeMatrix(x);
freeMatrix(AA);
freeMatrix(bb);
freeLcpSolve(vx);
}
int main()
{
#ifndef ONLINE_JUDGE
freopen("input.txt", "rt", stdin);
#endif
scanf("%d\n%s", &N, &A);
memcpy(A + N, A, N);
N = 2 * N;
A[N] = A[N + 1] = A[N + 2] = 0;
int i;
for (i = 0; i < N; i++)
S[i] = A[i] - 'A';
suffixArray(S, SA, N, 26);
for (i = 0; i < N; i++)
printf("%d ", SA[i]);
printf("\n");
LCP = lcp(SA, A, N);
double answer = 0;
if (LCP)
{
for (i = 0; i < N; i++)
{
printf("%d ", LCP[i]);
answer += LCP[i];
}
printf("\n");
answer /= N - 1;
printf("%.3lf\n", answer);
}
return 0;
}
void dSolveLCPBasic (int n, dReal *A, dReal *x, dReal *b,
dReal *w, int nub, dReal *lo, dReal *hi)
{
dAASSERT (n>0 && A && x && b && w && nub == 0);
int i,k;
int nskip = dPAD(n);
dReal *L = (dReal*) ALLOCA (n*nskip*sizeof(dReal));
dReal *d = (dReal*) ALLOCA (n*sizeof(dReal));
dReal *delta_x = (dReal*) ALLOCA (n*sizeof(dReal));
dReal *delta_w = (dReal*) ALLOCA (n*sizeof(dReal));
dReal *Dell = (dReal*) ALLOCA (n*sizeof(dReal));
dReal *ell = (dReal*) ALLOCA (n*sizeof(dReal));
dReal *tmp = (dReal*) ALLOCA (n*sizeof(dReal));
dReal **Arows = (dReal**) ALLOCA (n*sizeof(dReal*));
int *p = (int*) ALLOCA (n*sizeof(int));
int *C = (int*) ALLOCA (n*sizeof(int));
int *dummy = (int*) ALLOCA (n*sizeof(int));
dLCP lcp (n,0,A,x,b,w,tmp,tmp,L,d,Dell,ell,tmp,dummy,dummy,p,C,Arows);
nub = lcp.getNub();
for (i=0; i<n; i++) {
w[i] = lcp.AiC_times_qC (i,x) - b[i];
if (w[i] >= 0) {
lcp.transfer_i_to_N (i);
}
else {
for (;;) {
// compute: delta_x(C) = -A(C,C)\A(C,i)
dSetZero (delta_x,n);
lcp.solve1 (delta_x,i);
delta_x[i] = 1;
// compute: delta_w = A*delta_x
dSetZero (delta_w,n);
lcp.pN_equals_ANC_times_qC (delta_w,delta_x);
lcp.pN_plusequals_ANi (delta_w,i);
delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i);
// find index to switch
int si = i; // si = switch index
int si_in_N = 0; // set to 1 if si in N
dReal s = -w[i]/delta_w[i];
if (s <= 0) {
dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s);
if (i < (n-1)) {
dSetZero (x+i,n-i);
dSetZero (w+i,n-i);
}
goto done;
}
for (k=0; k < lcp.numN(); k++) {
if (delta_w[lcp.indexN(k)] < 0) {
dReal s2 = -w[lcp.indexN(k)] / delta_w[lcp.indexN(k)];
if (s2 < s) {
s = s2;
si = lcp.indexN(k);
si_in_N = 1;
}
}
}
for (k=0; k < lcp.numC(); k++) {
if (delta_x[lcp.indexC(k)] < 0) {
dReal s2 = -x[lcp.indexC(k)] / delta_x[lcp.indexC(k)];
if (s2 < s) {
s = s2;
si = lcp.indexC(k);
si_in_N = 0;
}
}
}
// apply x = x + s * delta_x
lcp.pC_plusequals_s_times_qC (x,s,delta_x);
x[i] += s;
lcp.pN_plusequals_s_times_qN (w,s,delta_w);
w[i] += s * delta_w[i];
// switch indexes between sets if necessary
if (si==i) {
w[i] = 0;
lcp.transfer_i_to_C (i);
break;
}
if (si_in_N) {
w[si] = 0;
lcp.transfer_i_from_N_to_C (si);
}
else {
x[si] = 0;
lcp.transfer_i_from_C_to_N (si);
}
}
}
}
done:
lcp.unpermute();
}
int main()
{
int cas = 0;
while (scanf("%s", s) && s[0] != '#')
{
int l = strlen(s);
for (int i = 0; i < l; i++) r[i] = s[i] - 'a' + 1;
r[l] = 0;
da(r, sa, l+1, 27);
calheight(r, sa, l);
init_rmq(l);
int ans = 0;
for (int i = 1; i <= l / 2; i++)
for (int j = 0; j + i < l; j+=i)
{
int k = lcp(j, j+i);
int x = k / i + 1;
if (x + 1 < ans) continue;
int aa = i - k % i;
bool flag = 0;
if (j >= aa)
{
int kk = lcp(j-aa, j+i-aa);
int xx = kk / i + 1;
if (xx > x) x = xx, flag = 1;
}
// if (j-aa >= 0 && lcp(j-aa, j+i-aa) >= (x+1) * (i-1)) x++, flag = 1;
if (ans < x)
{
ans = x;
cnt = 0;
zz[cnt] = flag ? j-aa : j;
zz1[cnt++] = i;
}
else if (ans == x)
{
zz[cnt] = flag ? j-aa : j;
zz1[cnt++] = i;
}
}
printf("Case %d: ", ++cas);
char cc = 'z';
if (ans == 1)
{
for (int i = 0; i < l; i++)
if (s[i] < cc) cc = s[i];
printf("%c\n", cc);
continue;
}
int start = zz[0];
int l0 = zz1[0];
for (int i = 0; i < cnt; i++)
{
int k = lcp(zz[i], zz[i]+zz1[i]);
int aa = k - k % zz1[i];
for (int j = zz[i]; j >= 0 && j >= zz[i]-aa; j--)
if (lcp(j, j+zz1[i]) < (ans-1) * zz1[i]) break;
else if (rank[j] < rank[start])
{
start = j;
l0 = zz1[i];
}
}
s[start+l0*ans] = '\0';
puts(s+start);
}
return 0;
}
/* Function: mdlOutputs =======================================================
* do the main optimization routine here
* no discrete states are considered
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
int_T i, j, Result, qinfeas=0;
real_T *z = ssGetOutputPortRealSignal(S,0);
real_T *w = ssGetOutputPortRealSignal(S,1);
real_T *I = ssGetOutputPortRealSignal(S,2);
real_T *exitflag = ssGetOutputPortRealSignal(S,3);
real_T *pivots = ssGetOutputPortRealSignal(S,4);
real_T *time = ssGetOutputPortRealSignal(S,5);
InputRealPtrsType M = ssGetInputPortRealSignalPtrs(S,0);
InputRealPtrsType q = ssGetInputPortRealSignalPtrs(S,1);
ptrdiff_t pivs, info, m, n, inc=1;
char_T T='N';
double total_time, alpha=1.0, tmp=-1.0, *x, *Mn, *qn, *r, *c, s=0.0, sn=0.0;
PT_Matrix pA; /* Problem data A = [M -1 q] */
PT_Basis pB; /* The basis */
T_Options options; /* options structure defined in lcp_matrix.h */
/* for RTW we do not need these variables */
#ifdef MATLAB_MEX_FILE
const char *fname;
mxArray *fval;
int_T nfields;
clock_t t1,t2;
#endif
UNUSED_ARG(tid); /* not used in single tasking mode */
/* default options */
options.zerotol = 1e-10; /* zero tolerance */
options.lextol = 1e-10; /* lexicographic tolerance - a small treshold to determine if values are equal */
options.maxpiv = INT_MAX; /* maximum number of pivots */
/* if LUMOD routine is chosen, this options refactorizes the basis after n steps using DGETRF
routine from lapack to avoid numerical problems */
options.nstepf = 50;
options.clock = 0; /* 0 or 1 - to print computational time */
options.verbose = 0; /* 0 or 1 - verbose output */
/* which routine in Basis_solve should solve a set of linear equations:
0 - corresponds to LUmod package that performs factorization in the form L*A = U. Depending on
the change in A factors L, U are updated. This is the fastest method.
1 - corresponds to DGESV simple driver
which solves the system AX = B by factorizing A and overwriting B with the solution X
2 - corresponds to DGELS simple driver
which solves overdetermined or underdetermined real linear systems min ||b - Ax||_2
involving an M-by-N matrix A, or its transpose, using a QR or LQ factorization of A. */
options.routine = 0;
options.timelimit = 3600; /* time limit in seconds to interrupt iterations */
options.normalize = 1; /* 0 or 1 - perform scaling of input matrices M, q */
options.normalizethres = 1e6;
/* If the normalize option is on, then the matrix scaling is performed
only if 1 norm of matrix M (maximum absolute column sum) is above this threshold.
This enforce additional control over normalization since it seems to be more
aggressive also for well-conditioned problems. */
#ifdef MATLAB_MEX_FILE
/* overwriting default options by the user */
if (ssGetSFcnParamsCount(S)==3) {
nfields = mxGetNumberOfFields(ssGetSFcnParam(S,2));
for(i=0; i<nfields; i++){
fname = mxGetFieldNameByNumber(ssGetSFcnParam(S,2), i);
fval = mxGetField(ssGetSFcnParam(S,2), 0, fname);
if ( strcmp(fname,"zerotol")==0 )
options.zerotol = mxGetScalar(fval);
if ( strcmp(fname,"lextol")==0 )
options.lextol = mxGetScalar(fval);
if ( strcmp(fname,"maxpiv")==0 ) {
if (mxGetScalar(fval)>=(double)INT_MAX)
options.maxpiv = INT_MAX;
else
options.maxpiv = (int_T)mxGetScalar(fval);
}
if ( strcmp(fname,"nstepf")==0 )
options.nstepf = (int_T)mxGetScalar(fval);
if ( strcmp(fname,"timelimit")==0 )
options.timelimit = mxGetScalar(fval);
if ( strcmp(fname,"clock")==0 )
options.clock = (int_T)mxGetScalar(fval);
if ( strcmp(fname,"verbose")==0 )
options.verbose = (int_T)mxGetScalar(fval);
if ( strcmp(fname,"routine")==0 )
options.routine = (int_T)mxGetScalar(fval);
if ( strcmp(fname,"normalize")==0 )
options.normalize = (int_T)mxGetScalar(fval);
if ( strcmp(fname, "normalizethres")==0 )
options.normalizethres = mxGetScalar(fval);
}
}
#endif
/* Normalize M, q to avoid numerical problems if possible
Mn = diag(r)*M*diag(c) , qn = diag(r)*q */
/* initialize Mn, qn */
Mn = (double *)ssGetPWork(S)[0];
qn = (double *)ssGetPWork(S)[1];
/* initialize vectors r, c */
r = (double *)ssGetPWork(S)[2];
c = (double *)ssGetPWork(S)[3];
/* initialize auxiliary vector x */
x = (double *)ssGetPWork(S)[4];
/* initialize to ones */
for (i=0; i<NSTATES(S); i++) {
r[i] = 1.0;
c[i] = 1.0;
}
m = NSTATES(S);
n = m*m;
/* write data to Mn = M */
memcpy(Mn, *M, n*sizeof(double));
/* write data to qn = q */
memcpy(qn, *q, m*sizeof(double));
/* check out the 1-norm of matrix M (maximum column sum) */
for (i=0; i<m; i++) {
sn = dasum(&m, &Mn[i*m], &inc);
if (sn>s) {
s = sn;
}
}
/* scale matrix M, q and write scaling factors to r (rows) and c (columns) */
if (options.normalize && s>options.normalizethres) {
NormalizeMatrix (m, m, Mn, qn, r, c, options);
}
/* Setup the problem */
pA = ssGetPWork(S)[5];
/* A(:,1:m) = M */
memcpy(pMAT(pA), Mn, n*sizeof(double));
/* A(:,1:m) = -A(:,1:m) */
dscal(&n, &tmp, pMAT(pA), &inc);
/* A(:,m+1) = -1 */
for(i=0;i<m;i++)
C_SEL(pA,i,m) = -1.0;
/* A(:,m+2) = q */
memcpy(&(C_SEL(pA,0,m+1)),qn,m*sizeof(double));
/* initialize basis */
pB = ssGetPWork(S)[6];
/* check if the problem is not feasible at the beginning */
for (i=0; i<m; i++) {
if (qn[i]<-options.zerotol) {
qinfeas = 1;
break;
}
}
/* Solve the LCP */
if (qinfeas) {
#ifdef MATLAB_MEX_FILE
t1 = clock();
#endif
/* main LCP rouinte */
Result = lcp(pB, pA, &pivs, options);
#ifdef MATLAB_MEX_FILE
t2 = clock();
total_time = ((double)(t2-t1))/CLOCKS_PER_SEC;
#else
total_time = -1;
#endif
} else {
pivs = 0;
total_time = 0;
Result = LCP_FEASIBLE;
}
#ifdef MATLAB_MEX_FILE
if (options.clock) {
printf("Time needed to perform pivoting:\n time= %i (%lf seconds)\n",
t2-t1,total_time);
printf("Pivots: %ld\n", pivs);
printf("CLOCKS_PER_SEC = %i\n",CLOCKS_PER_SEC);
}
#endif
/* initialize values to 0 */
for(i=0;i<NSTATES(S);i++)
{
w[i] = 0.0;
z[i] = 0.0;
I[i] = 0.0;
}
/* for a feasible basis, compute the solution */
if ( Result == LCP_FEASIBLE || Result == LCP_PRETERMINATED )
{
#ifdef MATLAB_MEX_FILE
t1 = clock();
#endif
info = Basis_Solve(pB, &(C_SEL(pA,0,m+1)), x, options);
for (j=0,i=0;i<Index_Length(pB->pW);i++,j++)
{
w[Index_Get(pB->pW,i)] = x[j];
/* add 1 due to matlab 1-indexing */
I[j] = Index_Get(pB->pW,i)+1;
}
for(i=0;i<Index_Length(pB->pZ);i++,j++)
{
/* take only positive values */
if (x[j] > options.zerotol ) {
z[Index_Get(pB->pZ,i)] = x[j];
}
/* add 1 due to matlab 1-indexing */
I[j] = Index_Get(pB->pZ, i)+m+1;
}
#ifdef MATLAB_MEX_FILE
t2 = clock();
total_time+=(double)(t2-t1)/(double)CLOCKS_PER_SEC;
if (options.clock) {
printf("Time in total needed to solve LCP: %lf seconds\n",
total_time + (double)(t2-t1)/(double)CLOCKS_PER_SEC);
}
#endif
if (options.normalize) {
/* do the backward normalization */
/* z = diag(c)*zn */
for (i=0; i<m; i++) {
z[i] = c[i]*z[i];
}
/* since the normalization does not compute w properly, we recalculate it from
* recovered z */
/* write data to Mn = M */
memcpy(Mn, *M, n*sizeof(double));
/* write data to qn = q */
memcpy(qn, *q, m*sizeof(double));
/* copy w <- q; */
dcopy(&m, qn, &inc, w, &inc);
/* compute w = M*z + q */
dgemv(&T, &m, &m, &alpha, Mn, &m, z, &inc, &alpha, w, &inc);
/* if w is less than eps, consider it as zero */
for (i=0; i<m; i++) {
if (w[i]<options.zerotol) {
w[i] = 0.0;
}
}
}
}
/* outputs */
*exitflag = (real_T )Result;
*pivots =(real_T)pivs;
*time = (real_T)total_time;
/* reset dimensions and values in basis for a recursive call */
Reinitialize_Basis(m, pB);
}
bool dSolveLCP (int n, dReal *A, dReal *x, dReal *b,
dReal *outer_w/*=nullptr*/, int nub, dReal *lo, dReal *hi, int *findex, bool earlyTermination)
{
dAASSERT (n>0 && A && x && b && lo && hi && nub >= 0 && nub <= n);
# ifndef dNODEBUG
{
// check restrictions on lo and hi
for (int k=0; k<n; ++k) dIASSERT (lo[k] <= 0 && hi[k] >= 0);
}
# endif
// if all the variables are unbounded then we can just factor, solve,
// and return
if (nub >= n) {
dReal *d = new dReal[n];
dSetZero (d, n);
int nskip = dPAD(n);
dFactorLDLT (A, d, n, nskip);
dSolveLDLT (A, d, b, n, nskip);
memcpy (x, b, n*sizeof(dReal));
delete[] d;
return true;
}
const int nskip = dPAD(n);
dReal *L = new dReal[ (n*nskip)];
dReal *d = new dReal[ (n)];
dReal *w = outer_w ? outer_w : (new dReal[n]);
dReal *delta_w = new dReal[ (n)];
dReal *delta_x = new dReal[ (n)];
dReal *Dell = new dReal[ (n)];
dReal *ell = new dReal[ (n)];
#ifdef ROWPTRS
dReal **Arows = new dReal* [n];
#else
dReal **Arows = nullptr;
#endif
int *p = new int[n];
int *C = new int[n];
// for i in N, state[i] is 0 if x(i)==lo(i) or 1 if x(i)==hi(i)
bool *state = new bool[n];
// create LCP object. note that tmp is set to delta_w to save space, this
// optimization relies on knowledge of how tmp is used, so be careful!
dLCP lcp(n,nskip,nub,A,x,b,w,lo,hi,L,d,Dell,ell,delta_w,state,findex,p,C,Arows);
int adj_nub = lcp.getNub();
// loop over all indexes adj_nub..n-1. for index i, if x(i),w(i) satisfy the
// LCP conditions then i is added to the appropriate index set. otherwise
// x(i),w(i) is driven either +ve or -ve to force it to the valid region.
// as we drive x(i), x(C) is also adjusted to keep w(C) at zero.
// while driving x(i) we maintain the LCP conditions on the other variables
// 0..i-1. we do this by watching out for other x(i),w(i) values going
// outside the valid region, and then switching them between index sets
// when that happens.
bool hit_first_friction_index = false;
for (int i=adj_nub; i<n; ++i) {
bool s_error = false;
// the index i is the driving index and indexes i+1..n-1 are "dont care",
// i.e. when we make changes to the system those x's will be zero and we
// don't care what happens to those w's. in other words, we only consider
// an (i+1)*(i+1) sub-problem of A*x=b+w.
// if we've hit the first friction index, we have to compute the lo and
// hi values based on the values of x already computed. we have been
// permuting the indexes, so the values stored in the findex vector are
// no longer valid. thus we have to temporarily unpermute the x vector.
// for the purposes of this computation, 0*infinity = 0 ... so if the
// contact constraint's normal force is 0, there should be no tangential
// force applied.
if (!hit_first_friction_index && findex && findex[i] >= 0) {
// un-permute x into delta_w, which is not being used at the moment
for (int j=0; j<n; ++j) delta_w[p[j]] = x[j];
// set lo and hi values
for (int k=i; k<n; ++k) {
dReal wfk = delta_w[findex[k]];
if (wfk == 0) {
hi[k] = 0;
lo[k] = 0;
}
else {
hi[k] = dFabs (hi[k] * wfk);
lo[k] = -hi[k];
}
}
hit_first_friction_index = true;
}
// thus far we have not even been computing the w values for indexes
// greater than i, so compute w[i] now.
w[i] = lcp.AiC_times_qC (i,x) + lcp.AiN_times_qN (i,x) - b[i];
// if lo=hi=0 (which can happen for tangential friction when normals are
// 0) then the index will be assigned to set N with some state. however,
// set C's line has zero size, so the index will always remain in set N.
// with the "normal" switching logic, if w changed sign then the index
// would have to switch to set C and then back to set N with an inverted
// state. this is pointless, and also computationally expensive. to
// prevent this from happening, we use the rule that indexes with lo=hi=0
// will never be checked for set changes. this means that the state for
// these indexes may be incorrect, but that doesn't matter.
// see if x(i),w(i) is in a valid region
if (lo[i]==0 && w[i] >= 0) {
lcp.transfer_i_to_N (i);
state[i] = false;
}
else if (hi[i]==0 && w[i] <= 0) {
lcp.transfer_i_to_N (i);
state[i] = true;
}
else if (w[i]==0) {
// this is a degenerate case. by the time we get to this test we know
// that lo != 0, which means that lo < 0 as lo is not allowed to be +ve,
// and similarly that hi > 0. this means that the line segment
// corresponding to set C is at least finite in extent, and we are on it.
// NOTE: we must call lcp.solve1() before lcp.transfer_i_to_C()
lcp.solve1 (delta_x,i,0,1);
lcp.transfer_i_to_C (i);
}
else {
// we must push x(i) and w(i)
for (;;) {
int dir;
dReal dirf;
// find direction to push on x(i)
if (w[i] <= 0) {
dir = 1;
dirf = REAL(1.0);
}
else {
dir = -1;
dirf = REAL(-1.0);
}
// compute: delta_x(C) = -dir*A(C,C)\A(C,i)
lcp.solve1 (delta_x,i,dir);
// note that delta_x[i] = dirf, but we wont bother to set it
// compute: delta_w = A*delta_x ... note we only care about
// delta_w(N) and delta_w(i), the rest is ignored
lcp.pN_equals_ANC_times_qC (delta_w,delta_x);
lcp.pN_plusequals_ANi (delta_w,i,dir);
delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i)*dirf;
// find largest step we can take (size=s), either to drive x(i),w(i)
// to the valid LCP region or to drive an already-valid variable
// outside the valid region.
int cmd = 1; // index switching command
int si = 0; // si = index to switch if cmd>3
dReal s = -w[i]/delta_w[i];
if (dir > 0) {
if (hi[i] < dInfinity) {
dReal s2 = (hi[i]-x[i])*dirf; // was (hi[i]-x[i])/dirf // step to x(i)=hi(i)
if (s2 < s) {
s = s2;
cmd = 3;
}
}
}
else {
if (lo[i] > -dInfinity) {
dReal s2 = (lo[i]-x[i])*dirf; // was (lo[i]-x[i])/dirf // step to x(i)=lo(i)
if (s2 < s) {
s = s2;
cmd = 2;
}
}
}
{
const int numN = lcp.numN();
for (int k=0; k < numN; ++k) {
const int indexN_k = lcp.indexN(k);
if (!state[indexN_k] ? delta_w[indexN_k] < 0 : delta_w[indexN_k] > 0) {
// don't bother checking if lo=hi=0
if (lo[indexN_k] == 0 && hi[indexN_k] == 0) continue;
dReal s2 = -w[indexN_k] / delta_w[indexN_k];
if (s2 < s) {
s = s2;
cmd = 4;
si = indexN_k;
}
}
}
}
{
const int numC = lcp.numC();
for (int k=adj_nub; k < numC; ++k) {
const int indexC_k = lcp.indexC(k);
if (delta_x[indexC_k] < 0 && lo[indexC_k] > -dInfinity) {
dReal s2 = (lo[indexC_k]-x[indexC_k]) / delta_x[indexC_k];
if (s2 < s) {
s = s2;
cmd = 5;
si = indexC_k;
}
}
if (delta_x[indexC_k] > 0 && hi[indexC_k] < dInfinity) {
dReal s2 = (hi[indexC_k]-x[indexC_k]) / delta_x[indexC_k];
if (s2 < s) {
s = s2;
cmd = 6;
si = indexC_k;
}
}
}
}
//static char* cmdstring[8] = {0,"->C","->NL","->NH","N->C",
// "C->NL","C->NH"};
//printf ("cmd=%d (%s), si=%d\n",cmd,cmdstring[cmd],(cmd>3) ? si : i);
// if s <= 0 then we've got a problem. if we just keep going then
// we're going to get stuck in an infinite loop. instead, just cross
// our fingers and exit with the current solution.
if (s <= REAL(0.0)) {
if (earlyTermination) {
if (!outer_w)
delete[] w;
delete[] L;
delete[] d;
delete[] delta_w;
delete[] delta_x;
delete[] Dell;
delete[] ell;
#ifdef ROWPTRS
delete[] Arows;
#endif
delete[] p;
delete[] C;
delete[] state;
return false;
}
// We shouldn't be overly aggressive about printing this warning,
// because sometimes it gets spammed if s is just a tiny bit beneath
// 0.0.
if (s < REAL(-1e-6)) {
dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",
(double)s);
}
if (i < n) {
dSetZero (x+i,n-i);
dSetZero (w+i,n-i);
}
s_error = true;
break;
}
// apply x = x + s * delta_x
lcp.pC_plusequals_s_times_qC (x, s, delta_x);
x[i] += s * dirf;
// apply w = w + s * delta_w
lcp.pN_plusequals_s_times_qN (w, s, delta_w);
w[i] += s * delta_w[i];
// void *tmpbuf;
// switch indexes between sets if necessary
switch (cmd) {
case 1: // done
w[i] = 0;
lcp.transfer_i_to_C (i);
break;
case 2: // done
x[i] = lo[i];
state[i] = false;
lcp.transfer_i_to_N (i);
break;
case 3: // done
x[i] = hi[i];
state[i] = true;
lcp.transfer_i_to_N (i);
break;
case 4: // keep going
w[si] = 0;
lcp.transfer_i_from_N_to_C (si);
break;
case 5: // keep going
x[si] = lo[si];
state[si] = false;
lcp.transfer_i_from_C_to_N (si, nullptr);
break;
case 6: // keep going
x[si] = hi[si];
state[si] = true;
lcp.transfer_i_from_C_to_N (si, nullptr);
break;
}
if (cmd <= 3) break;
} // for (;;)
} // else
if (s_error) {
break;
}
} // for (int i=adj_nub; i<n; ++i)
lcp.unpermute();
if (!outer_w)
delete[] w;
delete[] L;
delete[] d;
delete[] delta_w;
delete[] delta_x;
delete[] Dell;
delete[] ell;
#ifdef ROWPTRS
delete[] Arows;
#endif
delete[] p;
delete[] C;
delete[] state;
return true;
}
int main()
{
// std::ios_base::sync_with_stdio(false);
#ifdef kimbbakar
freopen ( "in.txt", "r", stdin );
freopen ( "out.txt", "w", stdout );
#endif
in(n),in(q);
for(int i=1;i<=n;i++){
in(val[i]);
cp[i-1]=val[i];
}
sort(cp,cp+n);
for(int i=1;i<=n;i++){
val[i] = lower_bound(cp,cp+n,val[i] )-cp;
}
int u,v;
for(int i=1;i<n;i++){
in(u),in(v);
lst[u].pb(v);
lst[v].pb(u);
}
dfs(1,0);
sp_table();
for(int i=0;i<q;i++){
in(u),in(v);
if(st[u]>st[v])
swap(u,v);
int p = lcp(u,v);
assert(p<=n);
qu[i].idx=i;
if(u!=p and v!=p){
qu[i].l=ed[u];
qu[i].r=st[v];
qu[i].lcp=p;
}
else{
qu[i].l=st[u];
qu[i].r=st[v];
qu[i].lcp=-1;
}
}
dv = 1000;
sort(qu,qu+q,cmp1);
int l=1,r=1;
cnt[ val[new_array[l]] ]++;
cc[ new_array[l]]++;
int tmp =1;
for(int i=0;i<q;i++){
while(l<qu[i].l) chk(l++,tmp) ;
while(qu[i].l<l) chk(--l,tmp);
while(r<qu[i].r) chk(++r,tmp) ;
while(qu[i].r<r) chk(r--,tmp);
res[qu[i].idx]=tmp;
if(qu[i].lcp!=-1 and cnt[val[qu[i].lcp] ]==0 )
res[qu[i].idx]++;
}
for(int i=0;i<q;i++)
pf("%d\n",res[i]);
return 0;
}
void construct_lcp_PHI(cache_config& config)
{
typedef int_vector<>::size_type size_type;
typedef int_vector<t_width> text_type;
const char* KEY_TEXT = key_text_trait<t_width>::KEY_TEXT;
int_vector_file_buffer<> sa_buf(config.file_map[constants::KEY_SA]);
size_type n = sa_buf.int_vector_size;
assert(n > 0);
if (1 == n) { // Handle special case: Input only the sentinel character.
int_vector<> lcp(1, 0);
store_to_cache(lcp, constants::KEY_LCP, config);
return;
}
// (1) Calculate PHI (stored in array plcp)
int_vector<> plcp(n, 0, sa_buf.width);
for (size_type i=0, r_sum=0, r=sa_buf.load_next_block(), sai_1 = 0; r_sum < n;) {
for (; i < r_sum+r; ++i) {
size_type sai = sa_buf[i-r_sum];
plcp[ sai ] = sai_1;
sai_1 = sai;
}
r_sum += r; r = sa_buf.load_next_block();
}
// (2) Load text from disk
text_type text;
load_from_cache(text, KEY_TEXT, config);
// (3) Calculate permuted LCP array (text order), called PLCP
size_type max_l = 0;
for (size_type i=0, l=0; i < n-1; ++i) {
size_type phii = plcp[i];
while (text[i+l] == text[phii+l]) {
++l;
}
plcp[i] = l;
if (l) {
max_l = std::max(max_l, l);
--l;
}
}
util::clear(text);
uint8_t lcp_width = bits::hi(max_l)+1;
// (4) Transform PLCP into LCP
std::string lcp_file = cache_file_name(constants::KEY_LCP, config);
osfstream lcp_out_buf(lcp_file, std::ios::binary | std::ios::app | std::ios::out); // open buffer for lcp
size_type bit_size = n*lcp_width;
lcp_out_buf.write((char*) &(bit_size), sizeof(bit_size)); // write size of vector
lcp_out_buf.write((char*) &(lcp_width),sizeof(lcp_width)); // write int_width of vector
size_type wb = 0; // bytes written into lcp int_vector
size_type buffer_size = 1000000; // buffer_size is a multiple of 8!
int_vector<> lcp_buf(buffer_size, 0, lcp_width);
lcp_buf[0] = 0;
sa_buf.reset(buffer_size);
size_type r = 0;// sa_buf.load_next_block();
for (size_type i=1, r_sum=0; r_sum < n;) {
for (; i < r_sum+r; ++i) {
size_type sai = sa_buf[i-r_sum];
lcp_buf[ i-r_sum ] = plcp[sai];
}
if (r > 0) {
size_type cur_wb = (r*lcp_buf.width()+7)/8;
lcp_out_buf.write((const char*)lcp_buf.data(), cur_wb);
wb += cur_wb;
}
r_sum += r; r = sa_buf.load_next_block();
}
if (wb%8) {
lcp_out_buf.write("\0\0\0\0\0\0\0\0", 8-wb%8);
}
lcp_out_buf.close();
register_cache_file(constants::KEY_LCP, config);
}
void dSolveLCPBasic (int n, dReal *A, dReal *x, dReal *b,
dReal *w, int nub, dReal *lo, dReal *hi)
{
dAASSERT (n>0 && A && x && b && w && nub == 0);
int i,k;
int nskip = dPAD(n);
ALLOCA (dReal,L,n*nskip*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (L == NULL) {
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,d,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (d == NULL) {
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,delta_x,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (delta_x == NULL) {
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,delta_w,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (delta_w == NULL) {
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,Dell,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (Dell == NULL) {
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,ell,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (ell == NULL) {
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal,tmp,n*sizeof(dReal));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (tmp == NULL) {
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (dReal*,Arows,n*sizeof(dReal*));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (Arows == NULL) {
UNALLOCA(tmp);
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (int,p,n*sizeof(int));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (p == NULL) {
UNALLOCA(Arows);
UNALLOCA(tmp);
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (int,C,n*sizeof(int));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (C == NULL) {
UNALLOCA(p);
UNALLOCA(Arows);
UNALLOCA(tmp);
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
ALLOCA (int,dummy,n*sizeof(int));
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (dummy == NULL) {
UNALLOCA(C);
UNALLOCA(p);
UNALLOCA(Arows);
UNALLOCA(tmp);
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
return;
}
#endif
dLCP lcp (n,0,A,x,b,w,tmp,tmp,L,d,Dell,ell,tmp,dummy,dummy,p,C,Arows);
nub = lcp.getNub();
for (i=0; i<n; i++) {
w[i] = lcp.AiC_times_qC (i,x) - b[i];
if (w[i] >= 0) {
lcp.transfer_i_to_N (i);
}
else {
for (;;) {
// compute: delta_x(C) = -A(C,C)\A(C,i)
dSetZero (delta_x,n);
lcp.solve1 (delta_x,i);
#ifdef dUSE_MALLOC_FOR_ALLOCA
if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY) {
UNALLOCA(dummy);
UNALLOCA(C);
UNALLOCA(p);
UNALLOCA(Arows);
UNALLOCA(tmp);
UNALLOCA(ell);
UNALLOCA(Dell);
UNALLOCA(delta_w);
UNALLOCA(delta_x);
UNALLOCA(d);
UNALLOCA(L);
return;
}
#endif
delta_x[i] = 1;
// compute: delta_w = A*delta_x
dSetZero (delta_w,n);
lcp.pN_equals_ANC_times_qC (delta_w,delta_x);
lcp.pN_plusequals_ANi (delta_w,i);
delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i);
// find index to switch
int si = i; // si = switch index
int si_in_N = 0; // set to 1 if si in N
dReal s = -w[i]/delta_w[i];
if (s <= 0) {
dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s);
if (i < (n-1)) {
dSetZero (x+i,n-i);
dSetZero (w+i,n-i);
}
goto done;
}
for (k=0; k < lcp.numN(); k++) {
if (delta_w[lcp.indexN(k)] < 0) {
dReal s2 = -w[lcp.indexN(k)] / delta_w[lcp.indexN(k)];
if (s2 < s) {
s = s2;
si = lcp.indexN(k);
si_in_N = 1;
}
}
}
for (k=0; k < lcp.numC(); k++) {
if (delta_x[lcp.indexC(k)] < 0) {
dReal s2 = -x[lcp.indexC(k)] / delta_x[lcp.indexC(k)];
if (s2 < s) {
s = s2;
si = lcp.indexC(k);
si_in_N = 0;
}
}
}
// apply x = x + s * delta_x
lcp.pC_plusequals_s_times_qC (x,s,delta_x);
x[i] += s;
lcp.pN_plusequals_s_times_qN (w,s,delta_w);
w[i] += s * delta_w[i];
// switch indexes between sets if necessary
if (si==i) {
w[i] = 0;
lcp.transfer_i_to_C (i);
break;
}
if (si_in_N) {
w[si] = 0;
lcp.transfer_i_from_N_to_C (si);
}
else {
x[si] = 0;
lcp.transfer_i_from_C_to_N (si);
}
}
}
}
done:
lcp.unpermute();
UNALLOCA (L);
UNALLOCA (d);
UNALLOCA (delta_x);
UNALLOCA (delta_w);
UNALLOCA (Dell);
UNALLOCA (ell);
UNALLOCA (tmp);
UNALLOCA (Arows);
UNALLOCA (p);
UNALLOCA (C);
UNALLOCA (dummy);
}
