(defn lin-solve [mat]
(let [width (count (first mat))
height (count mat)]
- (loop [pivot 0 m (vec (map vec mat)) swap-acc ()]
+ (loop [pivot 0 m (vec (map vec mat))]
(if (>= pivot (max (dec width) height))
- [m swap-acc]
- (let [[col row] (search-non-zero mat pivot)]
- (if (nil? col)
- m
- (recur (inc pivot)
- (sweep1 (swap pivot row
- (map (partial swap pivot col) m))
- pivot)
- (cons [pivot col] swap-acc)
- )))))))
-
-;
-; __ 0 |\ 2 3
-; |__>-------| >o--------+
-; 1|/ |
-; |
-; | __
-; +---------<__|
-; 4 5
-; nodes:
-; [0 in] [1 mid] [2 mid] [3 mid] [4 mid] [5 out]
-; edges:
-; [hor 0 1] [not1 1 2] [hor 2 3] [ver 3 4] [hor 4 5]
-;
-; [x0 y0 x1 y1 x2 y2 x3 y3 x4 y4 x5 y5 -const]
-; [ 1 0 0 0 0 0 0 0 0 0 0 0 (- inport-x)] ; [0 in]
-; [ 0 0 0 0 0 0 0 0 0 0 1 0 (- outport-x)] ; [5 out]
-; [ 0 1 0 -1 0 0 0 0 0 0 0 0 0] ; [hor 0 1]
-; [ 0 0 -1 0 1 0 0 0 0 0 0 0 not1-width] ; [not1 1 2]
-; [ 0 0 0 1 0 -1 0 0 0 0 0 0 0] ; [not1 1 2]
-; [ 0 0 0 0 0 1 0 -1 0 0 0 0 0] ; [hor 2 3]
-; [ 0 0 0 0 0 0 1 0 -1 0 0 0 0] ; [ver 3 4]
-; [ 0 0 0 0 0 0 0 0 0 1 0 -1 0] ; [hor 4 5]
-
-(def f-table {'+ +, '- -, '* *, '/ /})
-
-(defn neval [expr arg env]
- (cond (isa? (class expr) Number) expr
- (= expr 'arg) arg
- (symbol? expr) (env expr)
- (= (first expr) 'arg) (nth arg (nth expr 1))
- :else (apply (f-table (first expr))
- (map #(neval % arg env) (rest expr))
- )))
+ m
+ (let [[row col] (search-non-zero mat pivot)]
+ (cond (nil? row) m
+ (not= col pivot) m
+ :else (recur (inc pivot)
+ (sweep1 (swap pivot row m)
+ pivot))))))))
(defn m*v [m v]
(map #(apply + (map * % v)) m))
(map #(cons 0 %) acc)
)))))
-(def *dxi* 0.001)
+; Lagrangean equation
+; D(d2 L o G[q]) - d1 L o G[q] = 0
+; (D d2 L o G[q])D G[q] = d1 L o G[q]
+; 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]
+; D^2q =
+; (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)
+; A = (d2 d2 L)^(-1) (d1 L - d0 d2 L - (d1 d2 L)I2)
+;(defn L->accer [L]
+; (fn [x] ; x -> [up t q qdot]
+; (t* (inverse ((partial (partial L 2) 2) x))
+; (t- ((partial L 1) x)
+; ((partial (partial L 2) 0) x)
+; (t* (partial (partial L 2) 1)
+; (nth L 3)
+; )))))
+(defn tref [tuple & idxs]
+ (reduce #(nth %1 (inc %2))
+ tuple idxs))
+
+(defn tfassoc [x idxs func]
+ (if (empty? idxs)
+ (func x)
+ (assoc x
+ (inc (first idxs))
+ (tfassoc (nth x (inc (first idxs)))
+ (rest idxs)
+ func))))
+
+(defn t+2 [x y]
+ (cond (and (not (coll? x))
+ (not (coll? y)))
+ (+ x y)
+
+ (= (first x) (first y))
+ (vec (cons (first x)
+ (map t+2 (rest x) (rest y))
+ ))
+
+ :else nil
+ ))
+
+(defn t+ [x & xs]
+ (reduce t+2 x xs))
+
+(defn t-2 [x y]
+ (cond (and (not (coll? x))
+ (not (coll? y)))
+ (- x y)
-(defn jaco [phys xis env]
- (let [xis (vec xis)
- xis+ (map #(assoc xis %1 (+ (nth xis %1) (/ *dxi* 2)))
- (range (count xis)))
- xis- (map #(assoc xis %1 (- (nth xis %1) (/ *dxi* 2)))
- (range (count xis)))]
- (map (fn [phy] (map #(/ (- (neval phy %1 env)
- (neval phy %2 env))
- *dxi*)
- xis+ xis-))
- phys)))
+ (= (first x) (first y))
+ (vec (cons (first x)
+ (map t-2 (rest x) (rest y))
+ ))
+
+ :else nil
+ ))
+
+(defn t-
+ ([x] (if (coll? x)
+ (vec (cons (first x)
+ (map t- (rest x))
+ ))
+ (- x)
+ ))
+ ([x & xs] (reduce t-2 x xs))
+ )
+
+(defn t*2 [x y]
+ (if (coll? y)
+ (let [type-pair [(and (coll? x) (first x))
+ (first y)
+ ]]
+ (if (or (= type-pair '[up down])
+ (= type-pair '[down up]))
+ (apply t+
+ (map t*2 (rest x) (rest y)))
+ (vec (cons (first y)
+ (map #(t*2 x %) (rest y))
+ ))))
+ (if (coll? x)
+ (vec (cons (first x)
+ (map #(t*2 % y) (rest x))
+ ))
+ (* x y)
+ )))
+
+; (t* a b c d) --> (t*2 a (t*2 b (t*2 c d))) ; right associative
+(defn t* [x & xs]
+ (let [[y & ys] (reverse (cons x xs))]
+ (reduce #(t*2 %2 %1) y ys)))
+
+(def *dx* 0.001)
+
+(defn partial-val [f idxs x]
+ (letfn [(rec [idxs]
+ (let [el (apply tref x idxs)]
+ (if (coll? el)
+ (vec (cons ('{up down, down up} (first x))
+ (map #(rec (conj idxs %))
+ (range (dec (count el)))
+ )))
+ (t* (/ 1.0 *dx*)
+ (t- (f (tfassoc x idxs #(+ % (/ *dx* 2))))
+ (f (tfassoc x idxs #(- % (/ *dx* 2))))
+ )))))]
+ (rec (vec idxs))
+ ))
; dx_i/dt = f_i(x)
; x^i(t+h) = x^i(t) + f^i(x(t+h))h ; implicit Eular method
; = x^i(t) + f^i(x(t))h - h*df^i(x(t))/dx^j*x^j(t)
(def *h* 0.02)
-(defn next-iter [phys xis env]
- (let [j (jaco phys xis env)
- lhs (m-m (i-mat (count xis))
- (s*m *h* j))
- rhs (map #(+ %1 %2 (- %3))
- xis
- (map #(* *h* (neval % xis env)) phys)
- (m*v (s*m *h* j) xis))
- [solved varswap] (lin-solve (map #(conj %1 %2)
- (map vec lhs) rhs))]
- (reduce #(swap (nth %2 0) (nth %2 1) %1)
- (map #(nth % (count xis)) solved)
- varswap)))
-
-; symbol type
-(defn stype [x]
- (cond (isa? x Number) 'num
- (symbol? x) 'symb
- :else (first x)
- ))
-
-; symbolic addtion 2 terms
-(defn s+2 [x0 x1]
- (if (= (stype x1) '+)
- (concat ['+ (s+2 x0 (first x1))]
- (rest x1))
- (case [(stype x0) (stype x1)]
- [num num ] (+ x0 x1)
- [up up ] ['up (map + (rest x0) (rest x1))]
- [down down] ['down (map + (rest x0) (rest x1))]
- )))
+(defn next-iter [phys xis]
+ (let [jaco (partial-val phys [] xis)
+ lhs (m-m (i-mat (dec (count xis)))
+ (s*m *h*
+ (vec (apply map vector ; transpose
+ (map rest (rest jaco))
+ ))))
+ rhs (rest (t+ xis
+ (t* *h* (phys xis))
+ (t- (t* *h* jaco xis))
+ ))
+ solved (lin-solve (map #(conj %1 %2)
+ (map vec lhs) rhs))]
+ (vec (cons (first xis)
+ (map #(nth % (count solved))
+ solved)))))
OSDN Git Service
