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 (defn m*v [m v]
  49   (map #(apply + (map * % v)) m))
  50
  51 (defn s*m [s m]
  52   (map (fn [v] (map #(* s %) v))
  53        m))
  54
  55 (defn m-m [m0 m1]
  56   (map #(map - %1 %2)
  57        m0 m1))
  58
  59 (defn i-mat [size]
  60   (loop [i size acc []]
  61     (if (<= i 0)
  62       acc
  63       (recur (dec i)
  64              (cons (cons 1 (take (count acc) (repeat 0)))
  65                    (map #(cons 0 %) acc)
  66                    )))))
  67
  68 ; Lagrangean equation
  69 ; D(d2 L o G[q]) - d1 L o G[q] = 0
  70 ; (D d2 L o G[q])D G[q] = d1 L o G[q]
  71 ; d0 d2 L o G[q] + (d1 d2 L o G[q])Dq + (d2 d2 L o G[q])D^2q = d1 L o G[q]
  72 ; D^2q =
  73 ;  (d2 d2 L o G[q])^(-1) (d1 L o G[q] - d0 d2 L o G[q] - (d1 d2 L o G[q])Dq)
  74 ; A = (d2 d2 L)^(-1) (d1 L - d0 d2 L - (d1 d2 L)I2)
  75 ;(defn L->accer [L]
  76 ;  (fn [x] ; x -> [up t q qdot]
  77 ;    (t* (inverse ((partial (partial L 2) 2) x))
  78 ;        (t- ((partial L 1) x)
  79 ;            ((partial (partial L 2) 0) x)
  80 ;            (t* (partial (partial L 2) 1)
  81 ;                (nth L 3)
  82 ;                )))))
  83 (def *dxi* 0.001)
  84
  85 ; 'jaco' will be rewritten more shortly by using 'deriv' function.
  86 (defn jaco [phys xis]
  87   (let [xis (vec xis)
  88         xis+ (map #(assoc xis %1 (+ (nth xis %1) (/ *dxi* 2)))
  89                   (range (count xis)))
  90         xis- (map #(assoc xis %1 (- (nth xis %1) (/ *dxi* 2)))
  91                   (range (count xis)))]
  92     (apply map vector ; transpose
  93                (map (fn [xis+1 xis-1]
  94                       (map (fn [p+ p-]
  95                              (/ (- p+ p-) *dxi*))
  96                            (phys xis+1)
  97                            (phys xis-1)
  98                            ))
  99                     xis+ xis-
 100                     ))))
 101
 102 ; dx_i/dt = f_i(x)
 103 ; x^i(t+h) = x^i(t) + f^i(x(t+h))h ; implicit Eular method
 104 ; x^i(t+h) = x^i(t) + f^i(x(t) + x(t+h) - x(t))h
 105 ; 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
 106 ; x~i(t+h) - h*df^i(x(t))/dx^j*x^j(t+h)
 107 ;  = x^i(t) + f^i(x(t))h - h*df^i(x(t))/dx^j*x^j(t)
 108 (def *h* 0.02)
 109
 110 (defn next-iter [phys xis]
 111   (let [j (jaco phys xis)
 112         lhs (m-m (i-mat (count xis))
 113                  (s*m *h* j))
 114         rhs (map #(+ %1 %2 (- %3))
 115                  xis
 116                  (map #(* *h* %) (phys xis))
 117                  (m*v (s*m *h* j) xis))
 118         [solved varswap] (lin-solve (map #(conj %1 %2)
 119                                          (map vec lhs) rhs))]
 120     (reduce #(swap (nth %2 0) (nth %2 1) %1)
 121             (map #(nth % (count xis)) solved)
 122             varswap)))