;;; SIMPLEX -- Simplex algorithm. (define (matrix-rows a) (vector-length a)) (define (matrix-columns a) (FLOATvector-length (vector-ref a 0))) (define (matrix-ref a i j) (FLOATvector-ref (vector-ref a i) j)) (define (matrix-set! a i j x) (FLOATvector-set! (vector-ref a i) j x)) (define (fuck-up) (fatal-error "This shouldn't happen")) (define (simplex a m1 m2 m3) (define *epsilon* 1e-6) (if (not (and (>= m1 0) (>= m2 0) (>= m3 0) (= (matrix-rows a) (+ m1 m2 m3 2)))) (fuck-up)) (let* ((m12 (+ m1 m2 1)) (m (- (matrix-rows a) 2)) (n (- (matrix-columns a) 1)) (l1 (make-vector n)) (l2 (make-vector m)) (l3 (make-vector m2)) (nl1 n) (iposv (make-vector m)) (izrov (make-vector n)) (ip 0) (kp 0) (bmax 0.0) (one? #f) (pass2? #t)) (define (simp1 mm abs?) (set! kp (vector-ref l1 0)) (set! bmax (matrix-ref a mm kp)) (do ((k 1 (+ k 1))) ((>= k nl1)) (if (FLOATpositive? (if abs? (FLOAT- (FLOATabs (matrix-ref a mm (vector-ref l1 k))) (FLOATabs bmax)) (FLOAT- (matrix-ref a mm (vector-ref l1 k)) bmax))) (begin (set! kp (vector-ref l1 k)) (set! bmax (matrix-ref a mm (vector-ref l1 k))))))) (define (simp2) (set! ip 0) (let ((q1 0.0) (flag? #f)) (do ((i 0 (+ i 1))) ((= i m)) (if flag? (if (FLOAT< (matrix-ref a (vector-ref l2 i) kp) (FLOAT- *epsilon*)) (begin (let ((q (FLOAT/ (FLOAT- (matrix-ref a (vector-ref l2 i) 0)) (matrix-ref a (vector-ref l2 i) kp)))) (cond ((FLOAT< q q1) (set! ip (vector-ref l2 i)) (set! q1 q)) ((FLOAT= q q1) (let ((qp 0.0) (q0 0.0)) (let loop ((k 1)) (if (<= k n) (begin (set! qp (FLOAT/ (FLOAT- (matrix-ref a ip k)) (matrix-ref a ip kp))) (set! q0 (FLOAT/ (FLOAT- (matrix-ref a (vector-ref l2 i) k)) (matrix-ref a (vector-ref l2 i) kp))) (if (FLOAT= q0 qp) (loop (+ k 1)))))) (if (FLOAT< q0 qp) (set! ip (vector-ref l2 i))))))))) (if (FLOAT< (matrix-ref a (vector-ref l2 i) kp) (FLOAT- *epsilon*)) (begin (set! q1 (FLOAT/ (FLOAT- (matrix-ref a (vector-ref l2 i) 0)) (matrix-ref a (vector-ref l2 i) kp))) (set! ip (vector-ref l2 i)) (set! flag? #t))))))) (define (simp3 one?) (let ((piv (FLOAT/ (matrix-ref a ip kp)))) (do ((ii 0 (+ ii 1))) ((= ii (+ m (if one? 2 1)))) (if (not (= ii ip)) (begin (matrix-set! a ii kp (FLOAT* piv (matrix-ref a ii kp))) (do ((kk 0 (+ kk 1))) ((= kk (+ n 1))) (if (not (= kk kp)) (matrix-set! a ii kk (FLOAT- (matrix-ref a ii kk) (FLOAT* (matrix-ref a ip kk) (matrix-ref a ii kp))))))))) (do ((kk 0 (+ kk 1))) ((= kk (+ n 1))) (if (not (= kk kp)) (matrix-set! a ip kk (FLOAT* (FLOAT- piv) (matrix-ref a ip kk))))) (matrix-set! a ip kp piv))) (do ((k 0 (+ k 1))) ((= k n)) (vector-set! l1 k (+ k 1)) (vector-set! izrov k k)) (do ((i 0 (+ i 1))) ((= i m)) (if (FLOATnegative? (matrix-ref a (+ i 1) 0)) (fuck-up)) (vector-set! l2 i (+ i 1)) (vector-set! iposv i (+ n i))) (do ((i 0 (+ i 1))) ((= i m2)) (vector-set! l3 i #t)) (if (positive? (+ m2 m3)) (begin (do ((k 0 (+ k 1))) ((= k (+ n 1))) (do ((i (+ m1 1) (+ i 1)) (sum 0.0 (FLOAT+ sum (matrix-ref a i k)))) ((> i m) (matrix-set! a (+ m 1) k (FLOAT- sum))))) (let loop () (simp1 (+ m 1) #f) (cond ((FLOAT<= bmax *epsilon*) (cond ((FLOAT< (matrix-ref a (+ m 1) 0) (FLOAT- *epsilon*)) (set! pass2? #f)) ((FLOAT<= (matrix-ref a (+ m 1) 0) *epsilon*) (let loop ((ip1 m12)) (if (<= ip1 m) (cond ((= (vector-ref iposv (- ip1 1)) (+ ip n -1)) (simp1 ip1 #t) (cond ((FLOATpositive? bmax) (set! ip ip1) (set! one? #t)) (else (loop (+ ip1 1))))) (else (loop (+ ip1 1)))) (do ((i (+ m1 1) (+ i 1))) ((>= i m12)) (if (vector-ref l3 (- i (+ m1 1))) (do ((k 0 (+ k 1))) ((= k (+ n 1))) (matrix-set! a i k (FLOAT- (matrix-ref a i k))))))))) (else (simp2) (if (zero? ip) (set! pass2? #f) (set! one? #t))))) (else (simp2) (if (zero? ip) (set! pass2? #f) (set! one? #t)))) (if one? (begin (set! one? #f) (simp3 #t) (cond ((>= (vector-ref iposv (- ip 1)) (+ n m12 -1)) (let loop ((k 0)) (cond ((and (< k nl1) (not (= kp (vector-ref l1 k)))) (loop (+ k 1))) (else (set! nl1 (- nl1 1)) (do ((is k (+ is 1))) ((>= is nl1)) (vector-set! l1 is (vector-ref l1 (+ is 1)))) (matrix-set! a (+ m 1) kp (FLOAT+ (matrix-ref a (+ m 1) kp) 1.0)) (do ((i 0 (+ i 1))) ((= i (+ m 2))) (matrix-set! a i kp (FLOAT- (matrix-ref a i kp)))))))) ((and (>= (vector-ref iposv (- ip 1)) (+ n m1)) (vector-ref l3 (- (vector-ref iposv (- ip 1)) (+ m1 n)))) (vector-set! l3 (- (vector-ref iposv (- ip 1)) (+ m1 n)) #f) (matrix-set! a (+ m 1) kp (FLOAT+ (matrix-ref a (+ m 1) kp) 1.0)) (do ((i 0 (+ i 1))) ((= i (+ m 2))) (matrix-set! a i kp (FLOAT- (matrix-ref a i kp)))))) (let ((t (vector-ref izrov (- kp 1)))) (vector-set! izrov (- kp 1) (vector-ref iposv (- ip 1))) (vector-set! iposv (- ip 1) t)) (loop)))))) (and pass2? (let loop () (simp1 0 #f) (cond ((FLOATpositive? bmax) (simp2) (cond ((zero? ip) #t) (else (simp3 #f) (let ((t (vector-ref izrov (- kp 1)))) (vector-set! izrov (- kp 1) (vector-ref iposv (- ip 1))) (vector-set! iposv (- ip 1) t)) (loop)))) (else (list iposv izrov))))))) (define (test) (simplex (vector (FLOATvector 0.0 1.0 1.0 3.0 -0.5) (FLOATvector 740.0 -1.0 0.0 -2.0 0.0) (FLOATvector 0.0 0.0 -2.0 0.0 7.0) (FLOATvector 0.5 0.0 -1.0 1.0 -2.0) (FLOATvector 9.0 -1.0 -1.0 -1.0 -1.0) (FLOATvector 0.0 0.0 0.0 0.0 0.0)) 2 1 1)) (define (main . args) (run-benchmark "simplex" simplex-iters (lambda (result) (equal? result '(#(4 1 3 2) #(0 5 7 6)))) (lambda () (lambda () (test)))))