(define pi 3.141592653589793)
;; select every nth element from a list
(define (element-n l n)
(if (< (length l) n)
'()
(pair (list-ref l (- n 1))
(element-n (drop l n) n))))
;; assuming path is a combination of q's and p's
(define (path-splitter path points)
(let* ((n (floor (/ (length path) points))))
(element-n (map first path) n)))
;; Euclidian distance between 2 2D particles, assuming coords come in as (x1, y1)
(define (compute-euc-dist particle1 particle2)
(let ((x1 (first particle1))
(x2 (first particle2))
(y1 (second particle1))
(y2 (second particle2)))
(expt (+ (expt (- x1 x2) 2) (expt (- y1 y2) 2)) 0.5)))
;; get angle theta between 2 2D particles
(define (compute-angle particle1 particle2)
(let* ((x1 (first particle1))
(x2 (first particle2))
(y1 (second particle1))
(y2 (second particle2))
(rx (- x2 x1))
(ry (- y2 y1))
(default-val (atan (/ ry rx))))
(if (> rx 0)
(atan (/ ry rx))
(if (and (< rx 0) (>= ry 0))
(+ (atan (/ ry rx)) pi)
(if (and (< rx 0) (< ry 0))
(- (atan (/ ry rx)) pi)
(if (and (= rx 0) (> ry 0))
(/ pi 2)
(if (and (= rx 0) (< ry 0))
(/ pi -2)
0.0)))))))
;; go from x y to radial reprsentation
(define (xy->radial xy)
(let ((r (compute-euc-dist xy '(0.0 0.0)))
(theta (compute-angle xy '(0.0 0.0))))
(list r theta)))
;; This probably exists already, but just to be sure
;; (define (gaussian-lnpdf val mu var)
;; (* -0.5 (+ (+ 1.8378770664093453 (log var) )
;; (/ (* (- val mu) (- val mu))
;; var) )) )
;; Go from elastic property to spring damping constant c in F = -c*v
(define (elastic->damping elastic k mass)
(* (* 2 (sqrt (* k mass))) (/ (- (log elastic)) (sqrt (+ (expt pi 2.0) (expt (log elastic) 2.0))))))
;; multiply number through a list
(define (mtimes n l)
(map (lambda (x) (* n x)) l))
;; multiply number through a list of lists (GOD)
(define (mtimes2 n ll)
(map (lambda (l) (mtimes n l)) ll))
;; add number through a list
(define (mplus n l)
(map (lambda (x) (+ n x)) l))
;; add number through a list of lists
(define (mplus2 n ll)
(map (lambda (l) (mplus n l)) ll))
;; add the lists of two lists together
(define addl
(lambda xs
(let ([lla (first xs)]
[llb (second xs)]
[args (rest (rest xs))])
(if (equal? args '())
(map (lambda (la lb) (map + la lb)) lla llb)
(fold (lambda (x y) (addl x y)) (addl lla llb) args)))))
(define (strip l)
(if (equal? l '())
'()
(append (first l) (strip (rest l))) ))
;; remove the element indexed by ref from the list l, kind of
;; the opposite of list-ref. Assume zero-indexing.
(define (remove-element l ref)
(append (take l ref) (drop l (+ ref 1))))
;; turn momenta into velocity
(define (p->v pl ml)
;; assuming p and m are both vectors, however
;; p is a Nx2 vector-list and m is a N vector
(map (lambda (pxy m) (list (/ (first pxy) m) (/ (second pxy) m)) ) pl ml))
;; turn forces into accelerations
(define (F->a Fl ml)
;; assuming F and m are both vectors, however
;; F is a Nx2 vector-list and m is a N vector
(map (lambda (Fxy m) (list (/ (first Fxy) m) (/ (second Fxy) m)) ) Fl ml))
;; turn velocty into momenta
(define (v->p vl ml)
;; assuming v and m are both vectors, however
;; v is a Nx2 vector-list and m is an N vector
(map (lambda (vxy m) (list (* (first vxy) m) (* (second vxy) m))) vl ml))
;; We don't want to have to remember property-lists by heart.
;; Since they will be in the form (("mass" 30) ("squiggler" #t)) and so on,
;; this function allows us to define general property-grabbing functions
(define (get-property property)
(lambda (property-list)
(second (first (filter (lambda (p) (equal? property (first p))) property-list)))))
;; will grab the (float) value of mass from a property list of a particle
(define get-mass (get-property "mass"))
(define get-elastic (get-property "elastic"))
;; will grab the (truth) value of squiggler-hood from a property list
(define get-squiggler (get-property "squiggler"))
;; will grab the (truth) value of spiraler-hood from a property list
(define get-spiraler (get-property "spiraler"))
;; will grab the (float) value of object size from a property list of a particle
(define get-size (get-property "size"))
(define (get-x q) (map first q))
(define (get-y q) (map second q))
(define (basic-collision pos mom lim)
(if (< pos (first lim))
(abs mom)
(if (> pos (second lim))
(- (abs mom))
mom)))
(define (collision-box xlim ylim)
(lambda (p q)
(let ((x (first q))
(y (second q))
(px (first p))
(py (second p)))
(list (basic-collision x px xlim) (basic-collision y py ylim)))))
;; Usually we're going to want several forces acting at once on the
;; different particles. We need a way to apply these individually
;; and then sum up the results. This function will take in the list of
;; forces, apply them to q and v in turn, and then return the summation
;; of Fx and Fy for each particle.
(define (apply-force-list F-list q v properties)
; we still want this to handle regular forces, so if it's not a list, just
; apply the force
(if (not (list? F-list))
(F-list q v properties)
(if (equal? F-list '())
(make-list (length q) '(0.0 0.0))
(addl ((first F-list) '() '() '() q v properties)
(apply-force-list (rest F-list) q v properties)))))
;; convenient function for handling multiple-interaction between pairs of particles.
;; takes in a force F which describes the interaction between any pair, and computes
;; the necessary N^2 interactions, spitting them out as ((Fx1, Fy1) (Fx2, Fy2), ...)
(define (multi-interaction-F F)
(lambda (q v properties)
(let* ((masses (map get-mass properties)))
(let ((n 0) (Fl '()))
(define (loop n Fl)
(if (< n (length q))
((lambda ()
(define main-q (list-ref q n))
(define main-v (list-ref v n))
(define main-properties (list-ref properties n))
(define rest-q (remove-element q n))
(define rest-v (remove-element v n))
(define rest-properties (remove-element properties n))
(define f-main-rest (map (lambda (q2 v2 properties2)
(F main-q q2 main-v v2 main-properties properties2))
rest-q rest-v rest-properties))
(define result (list (sum (map first f-main-rest)) (sum (map second f-main-rest))))
(loop (+ n 1) (pair result Fl))))
(reverse Fl)))
(loop n Fl)))))
;;integrator step, using Runge-Kutta (RK4)
;; No gradient methods necessary for Newton.
;; Notice this is written as p and q rather than x and v so
;; it will be more easily integrated into a single framework with
;; Hamiltonian dynamics.
;; Note that F is a function of q, v, properties and possibly t
;; F is a procedure that takes in vector-lists q, v and properties
;; and spits out a vector-list of forces in the x and
;; y directions, i.e. ((Fx1, Fy1), (Fx2, Fy2), ...) *sigh*.
;; Possibly it would be better for F to take in particles.
;; but that would require using "set!" and such.
;; a = F/m
;; v = integrate a
;; x = integrate v
;; The noise model adds the appropriate noise at the end of
;; this deterministic procedure.
;; NOTETOSELF: There must be a simpler way to do this using
;; some abstraction of a state evaluation.
(define rk-newtonian
(lambda (q0 v0 Fl properties steps dt noise)
(if (= steps 0)
'()
(let* ((masses (map get-mass properties))
;(v0 (p->v p0 masses))
(a0 (F->a (apply-force-list Fl q0 v0 properties) masses))
(a_q v0)
(a_v a0)
(b_q (addl v0 (mtimes2 (/ dt 2) a_v)))
(b_v (F->a (apply-force-list Fl (addl q0 (mtimes2 (/ dt 2) a_q))
(addl v0 (mtimes2 (/ dt 2) a_v))
properties)
masses))
(c_q (addl v0 (mtimes2 (/ dt 2) b_v)))
(c_v (F->a (apply-force-list Fl (addl q0 (mtimes2 (/ dt 2) b_q))
(addl v0 (mtimes2 (/ dt 2) b_v))
properties)
masses))
(d_q (addl v0 (mtimes2 dt c_v)))
(d_v (F->a (apply-force-list Fl (addl q0 (mtimes2 dt c_q))
(addl v0 (mtimes2 dt c_v))
properties)
masses))
(q1 (addl q0 (mtimes2 (/ dt 6)
(addl a_q
(mtimes2 2 b_q)
(mtimes2 2 c_q)
d_q))))
(v1 (addl v0 (mtimes2 (/ dt 6)
(addl a_v
(mtimes2 2 b_v)
(mtimes2 2 c_v)
d_v))))
;(v1 (add-noise v1 noise))
;(p1 (v->p v1 masses))
)
(pair (list q1 v1)
(rk-newtonian q1 v1 Fl properties (- steps 1) dt noise))))))
;; ======================== List of Forces ===================================
;; Force #0: nothing
(define (null-F q v properties)
(make-list (length q) '(0.0 0.0)))
;; Force #1: damped harmonic oscillator, one-dimensional
(define (damped-harmonic-F q v)
(let* ((x-vec (map first q))
(vx-vec (map first v))
(centerpoint -50)
(Fx (map + (map - (mtimes 0.1 vx-vec)) (map - (mplus centerpoint x-vec))))
(Fy (make-list (length x-vec) 0.0))
)
(zip Fx Fy)))
;; Force #2: gravitational force between two particles
(define (two-particle-grav-F q1 q2 v1 v2 properties1 properties2)
(let* ((r (compute-euc-dist q1 q2))
(theta (compute-angle q1 q2))
(mass1 (get-mass properties1))
(mass2 (get-mass properties2))
(F (* 100 (/ (* mass1 mass2) (expt r 2.0))))
(Fx (* F (cos theta)))
(Fy (* F (sin theta))))
(list Fx Fy)))
;; Force #3: gravitational force between multiple particles
(define gravitational-F (multi-interaction-F two-particle-grav-F))
;; Force #4: Squigglers. The force between them and non-squigglers is
;; proportional to the inverse of the distance, and the direction
;; oscillates as a function of the distance
(define (two-particle-squiggler-F q1 q2 v1 v2 properties1 properties2)
(let* ((r (compute-euc-dist q1 q2))
(theta (compute-angle q1 q2))
(is-squiggler1 (get-squiggler properties1))
(is-squiggler2 (get-squiggler properties2))
(F (/ 10000 (expt r 2.0)))
(Fy (* -1.0 (* F (sin r))))
(Fx (* F (cos r))))
(if (and is-squiggler1 is-squiggler2)
(list Fx Fy)
'(0.0 0.0))))
;; Force #5: multi-particle squiggle interaction
(define squiggler-F (multi-interaction-F two-particle-squiggler-F))
;; Force #6: Spiralers. The force between them and non-spirals is proportional
;; to the inverse of the distance, and the direction is perpendicular.
(define (two-particle-spiraler-F q1 q2 v1 v2 properties1 properties2)
(let* ((r (compute-euc-dist q1 q2))
(theta (compute-angle q1 q2))
(is-spiraler1 (get-spiraler properties1))
(is-spiraler2 (get-spiraler properties2))
(F (/ 10000 (expt r 2.0)))
(Fx (* -1.0 (* F (sin theta))))
(Fy (* F (cos theta))))
(if (and is-spiraler1 (not is-spiraler2))
(list Fx Fy)
'(0.0 0.0))))
;; Force #7: multi-particle spiral interaction
(define spiraler-F (multi-interaction-F two-particle-spiraler-F))
;; Force #8: global force. Contains all particles in a box
(define (box-F q v properties)
;; create a 'collision box' global perimeter
;; It may be the case that collisions are better handled
;; as 'events' in which the velocities are switched, as
;; opposed to some sort of Force-law treatment.
(let* ((k 100000.0) ;; strength of box, basically treat it like powerful tiny springs
(xlim0 100.0)
(xlim1 400.0)
(ylim0 100.0)
(ylim1 400.0)
(xvec (get-x q))
(yvec (get-y q))
(Fx (map (lambda (pos) (if (< pos xlim0) (* (- k) (- pos xlim0)) (if (> pos xlim1) (* (- k) (- pos xlim1)) 0.0))) xvec))
(Fy (map (lambda (pos) (if (< pos ylim0) (* (- k) (- pos ylim0)) (if (> pos ylim1) (* (- k) (- pos ylim1)) 0.0))) yvec)))
(zip Fx Fy)))
;; Force #9: collision forces. Each particle gets a tiny strong spring attached
;; to it. Requires a notion of object size.
(define (two-particle-collision-F q1 q2 v1 v2 properties1 properties2)
(let* ((k 200000.0) ; spring strength
(elastic 0.1)
(r (compute-euc-dist q1 q2))
(theta (compute-angle q1 q2))
(mass1 (get-mass properties1))
(e (get-elastic properties1))
(c (elastic->damping e k mass1))
(vr (+ (* (- (first v1) (first v2)) (cos theta)) (* (- (second v1) (second v2)) (sin theta))))
(size1 (get-size properties1))
(size2 (get-size properties2))
(impinge (- r (+ size1 size2))))
(if (< impinge 0)
;(let* ((F (* k impinge))
(let* ((F (+ (* (- c) vr) (* k impinge)))
(Fx (* F (cos theta)))
(Fy (* F (sin theta))))
(list Fx Fy))
'(0.0 0.0))))
;; Force #10: multi-collision formulation.
(define collision-F (multi-interaction-F two-particle-collision-F))
;; ====================== END of list of Forces =============================
(define F0 null-F)
(define F1 squiggler-F)
(define F2 gravitational-F)
(define F3 spiraler-F)
(define F4 box-F)
(define F5 collision-F)
;; Other write to file methods throw errors if the file exists.
;; This makes sure that if the file exists, it gets erased/written over
(define (write-to-file output filename)
(begin
(if (file-exists? filename)
(delete-file filename)
'())
(let ((output-port (open-output-file filename)))
(write output output-port))))
;; observation noise function
(define (add-noise l observation-noise)
(if (not (list? (first l)))
(map (lambda (x) (gaussian x observation-noise)) l)
(map (lambda (x) (add-noise x observation-noise)) l)))
;
;; ====================== Forces in inference form ==========================
;; (define (guessed-damped-harmonic-F q v)
;; (let* ((x-vec (map first q))
;; (vx-vec (map first v))
;; (centerpoint (gaussian -47 2))
;; (Fx (map + (map - (mtimes 0.1 vx-vec)) (map - (mplus centerpoint x-vec))))
;; (Fy (make-list (length x-vec) 0.0))
;; )
;; (zip Fx Fy)))
;; ===================== END of Forces in inference form ===================
;; ======= INITIALIZATION of position, momemtum and other parameters =======
;; starting positions and momentum
(define q0 '((200.0 200.0)
(100.0 200.0)))
(define v0 '((0.0 0.0)
(1000.0 0.0)))
;; particle properties ((p1_1, p1_2, p1_3,...), (p2_1, p2_2, p2_3,...),...)
;; properties can be things like charge, binary features, etc.
;; NOTE: Mass the is first property of a body!
(define properties '((("mass" 2.0) ("size" 12.0) ("elastic" 0.1))
(("mass" 1.0) ("size" 12.0) ("elastic" 0.1))))
(define dt 0.001)
(define observation-noise 0.0)
(define path-length 300 )
; the actual path is too long for efficient inference
; so we pick points along the path to condition on.
(define num-inference-points 100)
;; ========================= END of INITIALIZATION =========================
;; ========================= BEGIN ACTUAL RUN ==============================
(define (my-church-force q v properties)
(make-list (length q) '(0.0 0.0)))
(define true-Fl (list my-church-force))
;; --- run the actual actual ------
(define observed-path (rk-newtonian q0 v0 true-Fl properties path-length dt 0.0))
(define output-filename "./physics/collision-ratio2_0-e0_1")
(define samples-filename "./physics/output-collision-ratio2_0-e0_1")
;; ======================= END ACTUAL RUN ==================================
;; ======================= BEGIN INFERENCE =================================
(define samples
(mh-query
10 1
(define inferred-Fl
(if (flip)
(list F5)
(list my-church-force)))
(define (noisy= x y noise)
(log-flip (- (gaussian-lnpdf (- x y) 0.0 noise)
(gaussian-lnpdf 0.0 0.0 noise))))
(define (noisy=* a b noise)
(if (and (list? a) (list? b))
(all (map (lambda (i j) (noisy=* i j noise)) a b))
(noisy= a b noise)))
(define mass1 (exp 0.2))
(define mass2 (exp 0.2))
(define elastic (uniform 0.01 0.99))
(define inferred-properties (list (list (list "mass" mass1) (list "size" 12) (list "elastic" elastic))
(list (list "mass" mass2) (list "size" 12) (list "elastic" elastic))))
(define inferred-path (rk-newtonian q0 (add-noise v0 30) inferred-Fl inferred-properties path-length dt observation-noise))
;(define inferred-path (rk-newtonian q0 (add-noise v0 30) Fl inferred-properties path-length dt observation-noise))
(define reduced-observation (path-splitter observed-path num-inference-points))
(define reduced-inferred (path-splitter inferred-path num-inference-points))
;; query over
(begin
(display inferred-Fl)
(display "\n")
(list mass1 mass2))
;; (begin
;; (display (list mass1 mass2 elastic))
;; (display "\n")
;; ;(display (time-difference stime (current-time)))
;; ;(display "\n")
;; (list mass1 mass2))
;inferred-path
;; conditioned on
(noisy=* (map first observed-path) (map first inferred-path) .1)
(noisy=* reduced-observation reduced-inferred .1)
;(equal? reduced-observation reduced-inferred)
;(noisy=* 2.0 (gaussian 1.0 1.0) .1)
;true
)
)
samples
References: