dynamics/src/math.clj

   1 (ns src.math)
   2 ;(alias 'cl 'clojure.core)
   3
   4 (defn neg-index [cnt x]
   5   (if (neg? x) (+ cnt x) x))
   6
   7 (defn swap [i j s]
   8   (let [[i j] (map (partial neg-index (count s)) [i j])]
   9     (assoc (assoc (vec s) i (nth s j))
  10            j (nth s i)
  11            )))
  12
  13 (defn search-non-zero [mat pivot]
  14   (let [width (count (first mat))
  15         height (count mat)]
  16     (first (for [j (range pivot (dec width))
  17                  i (range pivot height)
  18                  :when (not= 0 (nth (nth mat i) j))]
  19                 [i j]))))
  20
  21 (defn sweep1 [mat pivot]
  22   (let [denom (nth (nth mat pivot) pivot)
  23         p-row (nth mat pivot)]
  24     (map (fn [i row]
  25            (if (= i pivot)
  26              (map #(/ % denom) row)
  27              (let [multiplier (/ (nth row pivot) denom)]
  28                (map #(- %1 %2) row (map #(* % multiplier) p-row))
  29                )))
  30          (iterate inc 0) mat)))
  31
  32 (defn lin-solve [mat]
  33   (let [width  (count (first mat))
  34         height (count mat)]
  35     (loop [pivot 0 m (vec (map vec mat)) swap-acc ()]
  36       (if (>= pivot (max (dec width) height))
  37         [m swap-acc]
  38         (let [[col row] (search-non-zero mat pivot)]
  39           (if (nil? col)
  40             m
  41             (recur (inc pivot)
  42                    (sweep1 (swap pivot row
  43                                  (map (partial swap pivot col) m))
  44                            pivot)
  45                    (cons [pivot col] swap-acc)
  46                    )))))))
  47
  48 ;
  49 ;  __ 0      |\  2        3
  50 ; |__>-------| >o--------+
  51 ;           1|/          |
  52 ;                        |
  53 ;                        |          __
  54 ;                        +---------<__|
  55 ;                       4         5
  56 ; nodes:
  57 ;  [0 in] [1 mid] [2 mid] [3 mid] [4 mid] [5 out]
  58 ; edges:
  59 ;  [hor 0 1] [not1 1 2] [hor 2 3] [ver 3 4] [hor 4 5]
  60 ;
  61 ; [x0 y0 x1 y1 x2 y2 x3 y3 x4 y4 x5 y5        -const]
  62 ; [ 1  0  0  0  0  0  0  0  0  0  0  0 (-  inport-x)] ; [0 in]
  63 ; [ 0  0  0  0  0  0  0  0  0  0  1  0 (- outport-x)] ; [5 out]
  64 ; [ 0  1  0 -1  0  0  0  0  0  0  0  0             0] ; [hor 0 1]
  65 ; [ 0  0 -1  0  1  0  0  0  0  0  0  0    not1-width] ; [not1 1 2]
  66 ; [ 0  0  0  1  0 -1  0  0  0  0  0  0             0] ; [not1 1 2]
  67 ; [ 0  0  0  0  0  1  0 -1  0  0  0  0             0] ; [hor 2 3]
  68 ; [ 0  0  0  0  0  0  1  0 -1  0  0  0             0] ; [ver 3 4]
  69 ; [ 0  0  0  0  0  0  0  0  0  1  0 -1             0] ; [hor 4 5]
  70
  71 (def f-table
  72   {'+ +, '- -, '* *, '/ /,
  73    'ref (fn [tuple idx]
  74           (nth tuple (inc idx))
  75           )})
  76
  77 (defn neval [expr arg env]
  78   (cond (isa? (class expr) Number) expr
  79         (= expr 'arg) arg
  80         (symbol? expr) (env expr)
  81         :else (apply (f-table (first expr))
  82                      (map #(neval % arg env) (rest expr))
  83                      )))
  84
  85 (defn m*v [m v]
  86   (map #(apply + (map * % v)) m))
  87
  88 (defn s*m [s m]
  89   (map (fn [v] (map #(* s %) v))
  90        m))
  91
  92 (defn m-m [m0 m1]
  93   (map #(map - %1 %2)
  94        m0 m1))
  95
  96 (defn i-mat [size]
  97   (loop [i size acc []]
  98     (if (<= i 0)
  99       acc
 100       (recur (dec i)
 101              (cons (cons 1 (take (count acc) (repeat 0)))
 102                    (map #(cons 0 %) acc)
 103                    )))))
 104
 105 (def *dxi* 0.001)
 106
 107 (defn jaco [phys xis env]
 108   (let [xis (vec xis)
 109         xis+ (map #(assoc xis %1 (+ (nth xis %1) (/ *dxi* 2)))
 110                   (range (count xis)))
 111         xis- (map #(assoc xis %1 (- (nth xis %1) (/ *dxi* 2)))
 112                   (range (count xis)))]
 113     (map (fn [phy] (map #(/ (- (neval phy (cons '_ %1) env)
 114                                (neval phy (cons '_ %2) env))
 115                             *dxi*)
 116                          xis+ xis-))
 117          phys)))
 118
 119 ; dx_i/dt = f_i(x)
 120 ; x^i(t+h) = x^i(t) + f^i(x(t+h))h ; implicit Eular method
 121 ; x^i(t+h) = x^i(t) + f^i(x(t) + x(t+h) - x(t))h
 122 ; x^i(t+h) = x^i(t) + f^i(x(t))h + df^i(x(t))/dx^j * (x^j(t+h) - x^j(t)) * h
 123 ; x~i(t+h) - h*df^i(x(t))/dx^j*x^j(t+h)
 124 ;  = x^i(t) + f^i(x(t))h - h*df^i(x(t))/dx^j*x^j(t)
 125 (def *h* 0.02)
 126
 127 (defn next-iter [phys xis env]
 128   (let [j (jaco phys xis env)
 129         lhs (m-m (i-mat (count xis))
 130                  (s*m *h* j))
 131         rhs (map #(+ %1 %2 (- %3))
 132                  xis
 133                  (map #(* *h* (neval % (cons '_ xis) env)) phys)
 134                  (m*v (s*m *h* j) xis))
 135         [solved varswap] (lin-solve (map #(conj %1 %2)
 136                                          (map vec lhs) rhs))]
 137     (reduce #(swap (nth %2 0) (nth %2 1) %1)
 138             (map #(nth % (count xis)) solved)
 139             varswap)))
 140
 141 ; symbol type
 142 (defn stype [x]
 143   (cond (isa? (class x) Number) 'num
 144         (symbol? x)             'symb
 145         :else                   (first x)
 146         ))
 147
 148 (defn stype-order [x]
 149   (let [order {'num  0
 150                'symb 1
 151                'up   2
 152                'down 3
 153                '+    4}]
 154     (get order (stype x))
 155     ))
 156
 157 ; symbolic addtion 2 terms
 158 (defn s+2 [x0 x1]
 159   (let [[t1 t2] [(stype x0) (stype x1)]]
 160     (cond (= [t1 t2] '[+    +   ]) (concat ['+] (rest x0) (rest x1))
 161           (= [t1 t2] '[symb +   ]) (concat ['+ x0] (rest x1))
 162           (=     t2         +    ) ['+ (s+2 x0 (first x1)) (rest x1)]
 163           (= [t1 t2] '[num  num ]) (+ x0 x1)
 164           (= [t1 t2] '[up   up  ]) (cons 'up (map + (rest x0) (rest x1)))
 165           (= [t1 t2] '[down down]) (cons 'down (map + (rest x0) (rest x1)))
 166           )))
 167
 168 ; symbolic add
 169 (defn s+ [& args]
 170   (letfn [(rec [x & xs]
 171             (if (empty? xs)
 172               x
 173               (s+2 x (apply rec xs))
 174               ))]
 175     (apply rec (sort-by stype-order args))
 176     ))
 177
 178