| 990 | // must have lo=0, hi=dInfinity, and nub=0. |
| 991 | |
| 992 | void dSolveLCPBasic (int n, dReal *A, dReal *x, dReal *b, |
| 993 | dReal *w, int nub, dReal *lo, dReal *hi) |
| 994 | { |
| 995 | dAASSERT (n>0 && A && x && b && w && nub == 0); |
| 996 | |
| 997 | int i,k; |
| 998 | int nskip = dPAD(n); |
| 999 | dReal *L = (dReal*) ALLOCA (n*nskip*sizeof(dReal)); |
| 1000 | dReal *d = (dReal*) ALLOCA (n*sizeof(dReal)); |
| 1001 | dReal *delta_x = (dReal*) ALLOCA (n*sizeof(dReal)); |
| 1002 | dReal *delta_w = (dReal*) ALLOCA (n*sizeof(dReal)); |
| 1003 | dReal *Dell = (dReal*) ALLOCA (n*sizeof(dReal)); |
| 1004 | dReal *ell = (dReal*) ALLOCA (n*sizeof(dReal)); |
| 1005 | dReal *tmp = (dReal*) ALLOCA (n*sizeof(dReal)); |
| 1006 | dReal **Arows = (dReal**) ALLOCA (n*sizeof(dReal*)); |
| 1007 | int *p = (int*) ALLOCA (n*sizeof(int)); |
| 1008 | int *C = (int*) ALLOCA (n*sizeof(int)); |
| 1009 | int *dummy = (int*) ALLOCA (n*sizeof(int)); |
| 1010 | |
| 1011 | dLCP lcp (n,0,A,x,b,w,tmp,tmp,L,d,Dell,ell,tmp,dummy,dummy,p,C,Arows); |
| 1012 | nub = lcp.getNub(); |
| 1013 | |
| 1014 | for (i=0; i<n; i++) { |
| 1015 | w[i] = lcp.AiC_times_qC (i,x) - b[i]; |
| 1016 | if (w[i] >= 0) { |
| 1017 | lcp.transfer_i_to_N (i); |
| 1018 | } |
| 1019 | else { |
| 1020 | for (;;) { |
| 1021 | // compute: delta_x(C) = -A(C,C)\A(C,i) |
| 1022 | dSetZero (delta_x,n); |
| 1023 | lcp.solve1 (delta_x,i); |
| 1024 | delta_x[i] = 1; |
| 1025 | |
| 1026 | // compute: delta_w = A*delta_x |
| 1027 | dSetZero (delta_w,n); |
| 1028 | lcp.pN_equals_ANC_times_qC (delta_w,delta_x); |
| 1029 | lcp.pN_plusequals_ANi (delta_w,i); |
| 1030 | delta_w[i] = lcp.AiC_times_qC (i,delta_x) + lcp.Aii(i); |
| 1031 | |
| 1032 | // find index to switch |
| 1033 | int si = i; // si = switch index |
| 1034 | int si_in_N = 0; // set to 1 if si in N |
| 1035 | dReal s = -w[i]/delta_w[i]; |
| 1036 | |
| 1037 | if (s <= 0) { |
| 1038 | dMessage (d_ERR_LCP, "LCP internal error, s <= 0 (s=%.4e)",s); |
| 1039 | if (i < (n-1)) { |
| 1040 | dSetZero (x+i,n-i); |
| 1041 | dSetZero (w+i,n-i); |
| 1042 | } |
| 1043 | goto done; |
| 1044 | } |
| 1045 | |
| 1046 | for (k=0; k < lcp.numN(); k++) { |
| 1047 | if (delta_w[lcp.indexN(k)] < 0) { |
| 1048 | dReal s2 = -w[lcp.indexN(k)] / delta_w[lcp.indexN(k)]; |
| 1049 | if (s2 < s) { |
nothing calls this directly
no test coverage detected