189 lines
7.5 KiB
Scheme
189 lines
7.5 KiB
Scheme
;;; 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)
|
|
;(define *epsilon* 0.000001)
|
|
(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)))))
|