Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

first commit

  • Loading branch information...
commit e01e975d98016e047b39e3815cc6ad2873d17a57 0 parents
@dharmatech authored
15 arbiter-key.sls
@@ -0,0 +1,15 @@
+
+(library (box2d-lite arbiter-key)
+
+ (export make-arbiter-key)
+
+ (import (rnrs))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-record-type arbiter-key
+ (fields body-1 body-1))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ )
388 arbiter.sls
@@ -0,0 +1,388 @@
+
+(library (box2d-lite arbiter)
+
+ (export make-arbiter
+
+ arbiter-contacts
+ arbiter-num-contacts
+ arbiter-body-1
+ arbiter-body-2
+ arbiter-friction
+
+ arbiter-contacts-set!
+ arbiter-num-contacts-set!
+ arbiter-body-1-set!
+ arbiter-body-2-set!
+ arbiter-friction-set!
+
+ is-arbiter
+ import-arbiter
+
+ create-arbiter
+ arbiter::pre-step
+ arbiter::apply-impulse
+
+ )
+
+ (import (rnrs)
+ (srfi :27)
+ (gl)
+ (agave glamour misc)
+ (dharmalab misc is-vector)
+ (box2d-lite util define-record-type)
+ (box2d-lite util say)
+ (box2d-lite util math)
+ (box2d-lite vec)
+ (box2d-lite body)
+ (box2d-lite feature-pair)
+ (box2d-lite contact)
+ (box2d-lite collide)
+ (box2d-lite world-parameters))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define MAX-POINTS 2)
+
+ (define-record-type++ arbiter
+ is-arbiter
+ import-arbiter
+ (fields (mutable contacts)
+ (mutable num-contacts)
+ (mutable body-1)
+ (mutable body-2)
+ (mutable friction))
+ (methods (update arbiter::update)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (create-arbiter b1 b2)
+
+ (let ((arb (make-arbiter (vector (create-contact)
+ (create-contact))
+ 0 0 0 0)))
+
+ (import-arbiter arb)
+
+ (is-body body-1)
+ (is-body body-2)
+
+ ;; (feature-pair-value-set! (contact-feature (vector-ref contacts 0))
+ ;; (random-integer 100000000))
+
+ ;; (feature-pair-value-set! (contact-feature (vector-ref contacts 1))
+ ;; (random-integer 100000000))
+
+ ;; (cond ((body-less-than? b1 b2)
+ ;; (body-1! b1)
+ ;; (body-2! b2))
+ ;; (else
+ ;; (body-1! b2)
+ ;; (body-2! b1)))
+
+ (body-1! b1)
+ (body-2! b2)
+
+ ;; (body-1! b2)
+ ;; (body-2! b1)
+
+ (num-contacts! (collide contacts body-1 body-2))
+
+ (friction! (sqrt (* body-1.friction body-2.friction)))
+
+ (glPointSize 4.0)
+
+ (glColor3f 1.0 0.0 0.0)
+
+ (gl-begin GL_POINTS
+
+ (do ((i 0 (+ i 1)))
+ ((>= i num-contacts))
+
+ (let ()
+
+ (is-vector contacts i)
+ (is-contact contacts.i)
+ (is-vec contacts.i.position)
+
+ (glVertex2f contacts.i.position.x contacts.i.position.y))))
+
+ (glPointSize 1.0)
+
+ arb))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (arbiter::update arb new-contacts num-new-contacts)
+
+ (import-arbiter arb)
+
+ ;; (say "arbiter::update")
+
+ (let ((merged-contacts (make-vector 2)))
+
+ (do ((i 0 (+ i 1)))
+ ((>= i num-new-contacts))
+
+ (let ()
+
+ (is-vector new-contacts i)
+ (is-contact new-contacts.i)
+ (is-feature-pair new-contacts.i.feature)
+
+ (is-vector merged-contacts i)
+ (is-contact merged-contacts.i)
+
+ (let ((k -1))
+
+ (is-vector contacts k)
+ (is-contact contacts.k)
+
+ (do ((j 0 (+ j 1))
+ (stop #f))
+ ((or (>= j num-contacts) stop))
+
+ (let ()
+
+ (is-vector contacts j)
+ (is-contact contacts.j)
+ (is-feature-pair contacts.j.feature)
+
+ ;; (say "new " (feature-pair-e new-contacts.i.feature))
+ ;; (say "old " (feature-pair-e contacts.j.feature))
+
+ (if (edges-equal? (feature-pair-e new-contacts.i.feature)
+ (feature-pair-e contacts.j.feature))
+ (begin
+ ;; (say "k = j " j)
+ (set! k j)
+ (set! stop #t)))))
+
+ (cond ((> k -1)
+ (merged-contacts.i! new-contacts.i)
+ (cond ((warm-starting)
+ (merged-contacts.i.pn! contacts.k.pn)
+ (merged-contacts.i.pt! contacts.k.pt)
+ (merged-contacts.i.pnb! contacts.k.pnb)
+
+ ;; (say-expr contacts.k.pn)
+
+ )
+ (else
+ (merged-contacts.i.pn! 0.0)
+ (merged-contacts.i.pt! 0.0)
+ (merged-contacts.i.pnb! 0.0))))
+ (else
+ (merged-contacts.i! new-contacts.i))))))
+
+ (do ((i 0 (+ i 1)))
+ ((>= i num-new-contacts))
+
+ (let ()
+
+ (is-vector contacts i)
+ (is-vector merged-contacts i)
+
+ (contacts.i! merged-contacts.i)))
+
+ ;; (say "contact[0].feature.e = "
+ ;; (feature-pair-e (contact-feature (vector-ref contacts 0))))
+ ;; (say "contact[1].feature.e = "
+ ;; (feature-pair-e (contact-feature (vector-ref contacts 1))))
+
+ (num-contacts! num-new-contacts)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (arbiter::pre-step arb inv-dt)
+ (import-arbiter arb)
+
+ (is-body body-1)
+ (is-body body-2)
+
+ ;; (say "arbiter::pre-step")
+
+ (let ((k-allowed-penetration 0.01)
+ (k-bias-factor (if (position-correction) 0.2 0.0)))
+
+ (do ((i 0 (+ i 1)))
+ ((>= i num-contacts))
+
+ (let ()
+
+ (is-vector contacts i)
+
+ (let ((c contacts.i))
+
+ (is-contact c)
+
+ (let ((r1 (v- c.position body-1.position))
+ (r2 (v- c.position body-2.position)))
+
+ (let ((rn1 (vec-dot r1 c.normal))
+ (rn2 (vec-dot r2 c.normal)))
+
+ (let ((k-normal (+ body-1.inv-mass
+ body-2.inv-mass
+ (* body-1.inv-i (- (vec-dot r1 r1) (* rn1 rn1)))
+ (* body-2.inv-i (- (vec-dot r2 r2) (* rn2 rn2))))))
+
+ (c.mass-normal! (/ 1.0 k-normal))
+
+ (let ((tangent (vxn c.normal 1.0)))
+
+ (let ((rt1 (vec-dot r1 tangent))
+ (rt2 (vec-dot r2 tangent)))
+
+ (let ((k-tangent
+ (+ body-1.inv-mass
+ body-2.inv-mass
+ (* body-1.inv-i (- (vec-dot r1 r1) (* rt1 rt1)))
+ (* body-2.inv-i (- (vec-dot r2 r2) (* rt2 rt2))))))
+
+ (c.mass-tangent! (/ 1.0 k-tangent))
+
+ (c.bias! (* -1
+ k-bias-factor
+ inv-dt
+ (min 0.0 (+ c.separation k-allowed-penetration))))
+
+ (if (accumulate-impulses)
+
+ (let ((p (v+ (n*v c.pn c.normal) (n*v c.pt tangent))))
+
+ ;; (say body-1.velocity)
+ ;; (say body-2.velocity)
+
+ (body-1.velocity!
+ (v- body-1.velocity (n*v body-1.inv-mass p)))
+
+ (body-1.angular-velocity!
+ (- body-1.angular-velocity (* body-1.inv-i (vxv r1 p))))
+
+ (body-2.velocity!
+ (v+ body-2.velocity (n*v body-2.inv-mass p)))
+
+ (body-2.angular-velocity!
+ (+ body-2.angular-velocity (* body-2.inv-i (vxv r2 p))))
+ )))))))))))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (arbiter::apply-impulse arb)
+
+ (import-arbiter arb)
+
+ (let ((b1 body-1)
+ (b2 body-2))
+
+ (is-body b1)
+ (is-body b2)
+
+ ;; (say-expr b2.velocity)
+
+ ;; (if (> (abs (vec-y b2.velocity)) 0.001)
+ ;; (say-expr (vec-y b2.velocity)))
+
+ ;; (if (> (abs (vec-y b2.velocity)) 0.001)
+ ;; (say-expr b2.velocity))
+
+ (do ((i 0 (+ i (+ i 1))))
+ ((>= i num-contacts))
+
+ (let ((c (vector-ref contacts i)))
+
+ (is-contact c)
+
+ (c.r1! (v- c.position b1.position))
+ (c.r2! (v- c.position b2.position))
+
+ (let ((dv (v- (v- (v+ b2.velocity (nxv b2.angular-velocity c.r2))
+ b1.velocity)
+ (nxv b1.angular-velocity c.r1))))
+
+ ;; (say-expr b2.velocity)
+
+ ;; (say-expr b2.angular-velocity)
+
+ ;; (say "c.r2 " c.r2)
+
+ (let ((vn (vec-dot dv c.normal)))
+
+ (let ((dpn (* c.mass-normal (+ (- vn) c.bias))))
+
+ ;; (say "c.mass-normal " c.mass-normal)
+
+ ;; (say "vn " vn)
+
+ ;; (say "dpn " dpn)
+
+ (if (accumulate-impulses)
+ (let ((pn0 c.pn))
+ (c.pn! (max (+ pn0 dpn) 0.0))
+ (set! dpn (- c.pn pn0))
+
+ ;; (say "c.pn " c.pn)
+
+ )
+ (set! dpn (max dpn 0.0)))
+
+ (let ((pn (n*v dpn c.normal)))
+
+ (b1.velocity! (v- b1.velocity (n*v b1.inv-mass pn)))
+
+ (b1.angular-velocity!
+ (- b1.angular-velocity (* b1.inv-i (vxv c.r1 pn))))
+
+ (b2.velocity! (v+ b2.velocity (n*v b2.inv-mass pn)))
+
+ (b2.angular-velocity!
+ (+ b2.angular-velocity (* b2.inv-i (vxv c.r2 pn))))
+
+ (set! dv (v- (v- (v+ b2.velocity
+ (nxv b2.angular-velocity c.r2))
+ b1.velocity)
+ (nxv b1.angular-velocity c.r1)))
+
+ (let ((tangent (vxn c.normal 1.0)))
+
+ (let ((vt (vec-dot dv tangent)))
+
+ (let ((dpt (* c.mass-tangent (- vt))))
+
+ ;; (say-expr dpt)
+
+ (if (accumulate-impulses)
+
+ (let ((max-pt (* friction c.pn))
+ (old-tangent-impulse c.pt))
+
+ ;; (say-expr max-pt)
+
+ (c.pt!
+ (clamp (+ old-tangent-impulse dpt) (- max-pt) max-pt))
+
+ (set! dpt (- c.pt old-tangent-impulse))
+
+ ;; (say-expr c.pt)
+ )
+
+ (let ((max-pt (* friction dpn)))
+
+ (set! dpt (clamp dpt (- max-pt) max-pt))))
+
+ (let ((pt (n*v dpt tangent)))
+
+ (b1.velocity! (v- b1.velocity (n*v b1.inv-mass pt)))
+ (b1.angular-velocity!
+ (- b1.angular-velocity (* b1.inv-i (vxv c.r1 pt))))
+
+ (b2.velocity! (v+ b2.velocity (n*v b2.inv-mass pt)))
+ (b2.angular-velocity!
+ (+ b2.angular-velocity (* b2.inv-i (vxv c.r2 pt))))
+
+ ))))))))))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ )
+
128 body.sls
@@ -0,0 +1,128 @@
+
+(library (box2d-lite body)
+
+ (export make-body
+
+ body-position
+ body-rotation
+ body-velocity
+ body-angular-velocity
+ body-force
+ body-torque
+ body-width
+ body-friction
+ body-mass
+ body-inv-mass
+ body-i
+ body-inv-i
+
+ body-position-set!
+ body-rotation-set!
+ body-velocity-set!
+ body-angular-velocity-set!
+ body-force-set!
+ body-torque-set!
+ body-width-set!
+ body-friction-set!
+ body-mass-set!
+ body-inv-mass-set!
+ body-i-set!
+ body-inv-i-set!
+
+ is-body
+ import-body
+
+ create-body
+ ;; body::set
+ )
+
+ (import (rnrs)
+ (box2d-lite util define-record-type)
+ (box2d-lite util math)
+ (box2d-lite vec))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-record-type++ body
+ is-body
+ import-body
+ (fields (mutable position)
+ (mutable rotation)
+ (mutable velocity)
+ (mutable angular-velocity)
+ (mutable force)
+ (mutable torque)
+ (mutable width)
+ (mutable friction)
+ (mutable mass)
+ (mutable inv-mass)
+ (mutable i)
+ (mutable inv-i))
+ (methods (set body::set)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (create-body)
+
+ (let ((b (make-body #f #f #f #f #f #f #f #f #f #f #f #f)))
+
+ (import-body b)
+
+ (position! (make-vec 0.0 0.0))
+ (rotation! 0.0)
+ (velocity! (make-vec 0.0 0.0))
+ (angular-velocity! 0.0)
+ (force! (make-vec 0.0 0.0))
+ (torque! 0.0)
+ (friction! 0.2)
+
+ (width! (make-vec 1.0 1.0))
+ (mass! FLT-MAX)
+ (inv-mass! 0.0)
+ (i! FLT-MAX)
+ (inv-i! 0.0)
+
+ b))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (body::set b w m)
+
+ (import-body b)
+
+ (is-vec width)
+
+ (position! (make-vec 0.0 0.0))
+ (rotation! 0.0)
+ (velocity! (make-vec 0.0 0.0))
+ (angular-velocity! 0.0)
+ (force! (make-vec 0.0 0.0))
+ (torque! 0.0)
+ (friction! 0.2)
+
+ (width! w)
+ (mass! m)
+
+ (if (< mass FLT-MAX)
+
+ (begin
+
+ (inv-mass! (/ 1.0 mass))
+
+ (i! (/ (* mass (+ (* width.x width.x) (* width.y width.y))) 12.0))
+
+ (inv-i! (/ 1.0 i)))
+
+ (begin
+
+ (inv-mass! 0.0)
+
+ (i! FLT-MAX)
+
+ (inv-i! 0.0)))
+
+ b)
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ )
80 clip-segment-to-line.sls
@@ -0,0 +1,80 @@
+
+(library (box2d-lite clip-segment-to-line)
+
+ (export clip-segment-to-line)
+
+ (import (rnrs)
+ (dharmalab misc is-vector)
+ (box2d-lite util say)
+ (box2d-lite vec)
+ (box2d-lite edge-numbers)
+ (box2d-lite clip-vertex)
+ (box2d-lite feature-pair)
+
+ (xitomatl debug)
+ )
+
+ (define (clip-segment-to-line v-out v-in normal offset clip-edge)
+
+ (define num-out 0)
+
+ (is-vector v-out num-out)
+ (is-clip-vertex v-out.num-out)
+ (is-feature-pair v-out.num-out.fp)
+ (is-edges v-out.num-out.fp.e)
+
+ (define-syntax v-in.0 (identifier-syntax (vector-ref v-in 0)))
+ (define-syntax v-in.1 (identifier-syntax (vector-ref v-in 1)))
+
+ (is-clip-vertex v-in.0)
+ (is-clip-vertex v-in.1)
+
+ ;; (say "********** clip-segment-to-line ********** ")
+
+ ;; (say "v-in[0].fp.e = " (feature-pair-e (clip-vertex-fp (vector-ref v-in 0))))
+ ;; (say "v-in[1].fp.e = " (feature-pair-e (clip-vertex-fp (vector-ref v-in 1))))
+
+ ;; (say-expr normal)
+ ;; (say-expr offset)
+ ;; (say-expr clip-edge)
+
+ (let ((distance-0 (- (vec-dot normal v-in.0.v) offset))
+ (distance-1 (- (vec-dot normal v-in.1.v) offset)))
+
+ ;; (say-expr distance-0)
+ ;; (say-expr distance-1)
+
+ ;; (say-expr (* distance-0 distance-1))
+
+ (if (<= distance-0 0.0)
+ (begin (v-out.num-out! v-in.0) (set! num-out (+ num-out 1))))
+
+ (if (<= distance-1 0.0)
+ (begin (v-out.num-out! v-in.1) (set! num-out (+ num-out 1))))
+
+ (if (< (* distance-0 distance-1) 0.0)
+
+ (let ((interp (/ distance-0 (- distance-0 distance-1))))
+
+ ;; (say "branch taken")
+
+ (v-out.num-out.v! (v+ v-in.0.v (n*v interp (v- v-in.1.v v-in.0.v))))
+
+ (cond ((> distance-0 0.0)
+ (v-out.num-out.fp! v-in.0.fp)
+ (v-out.num-out.fp.e.in-edge-1! clip-edge)
+ (v-out.num-out.fp.e.in-edge-2! NO-EDGE))
+
+ (else
+ (v-out.num-out.fp! v-in.1.fp)
+ (v-out.num-out.fp.e.out-edge-1! clip-edge)
+ (v-out.num-out.fp.e.out-edge-2! NO-EDGE)))
+
+ (set! num-out (+ num-out 1)))))
+
+ ;; (say "v-out[0].fp.e = " (feature-pair-e (clip-vertex-fp (vector-ref v-out 0))))
+ ;; (say "v-out[1].fp.e = " (feature-pair-e (clip-vertex-fp (vector-ref v-out 1))))
+
+ num-out)
+
+ )
43 clip-vertex.sls
@@ -0,0 +1,43 @@
+
+(library (box2d-lite clip-vertex)
+
+ (export make-clip-vertex
+
+ clip-vertex-v
+ clip-vertex-fp
+
+ clip-vertex-v-set!
+ clip-vertex-fp-set!
+
+ is-clip-vertex
+ import-clip-vertex
+
+ create-clip-vertex
+ )
+
+ (import (rnrs)
+ (box2d-lite util define-record-type)
+ (box2d-lite vec)
+ (box2d-lite feature-pair)
+ )
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-record-type++ clip-vertex
+ is-clip-vertex
+ import-clip-vertex
+ (fields (mutable v)
+ (mutable fp))
+ (methods))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (create-clip-vertex)
+ (make-clip-vertex
+ (make-vec 0.0 0.0)
+ (create-feature-pair)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ )
+
325 collide.sls
@@ -0,0 +1,325 @@
+
+(library (box2d-lite collide)
+
+ (export collide)
+
+ (import (rnrs)
+ (dharmalab misc is-vector)
+ (box2d-lite util say)
+ (box2d-lite vec)
+ (box2d-lite mat)
+ (box2d-lite body)
+ (box2d-lite edge-numbers)
+ (box2d-lite contact)
+ (box2d-lite clip-vertex)
+ (box2d-lite feature-pair)
+ (box2d-lite compute-incident-edge)
+ (box2d-lite clip-segment-to-line)
+ (xitomatl debug)
+ )
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define FACE-A-X 'FACE-A-X)
+ (define FACE-A-Y 'FACE-A-Y)
+ (define FACE-B-X 'FACE-B-X)
+ (define FACE-B-Y 'FACE-B-Y)
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (collide contacts body-a body-b)
+
+ (is-body body-a)
+ (is-body body-b)
+
+ ;; (display "collide ")
+
+ (let ((ha (n*v 0.5 body-a.width))
+ (hb (n*v 0.5 body-b.width))
+
+ (pos-a body-a.position)
+ (pos-b body-b.position)
+
+ (rot-a (angle->mat body-a.rotation))
+ (rot-b (angle->mat body-b.rotation)))
+
+ (is-vec ha)
+ (is-vec hb)
+
+ (is-mat rot-a)
+ (is-mat rot-b)
+
+ (let ((rot-at (rot-a.transpose))
+ (rot-bt (rot-b.transpose))
+
+ (dp (v- pos-b pos-a)))
+
+ (let* ((da (m*v rot-at dp))
+ (db (m*v rot-bt dp))
+
+ (abs-c (mat::abs (m* rot-at rot-b)))
+
+ (abs-ct (mat::transpose abs-c)))
+
+ (is-vec da)
+ (is-vec db)
+
+ (let ((face-a (v-v (vec::abs da) ha (m*v abs-c hb)))
+
+ (face-b (v-v (vec::abs db) (m*v abs-ct ha) hb)))
+
+ (is-vec face-a)
+ (is-vec face-b)
+
+ ;; (say "face-a " face-a)
+
+ (if (or (> face-a.x 0.0)
+ (> face-a.y 0.0)
+ (> face-b.x 0.0)
+ (> face-b.y 0.0))
+
+ 0
+
+ (let ((axis #f)
+ (separation #f)
+ (normal #f)
+
+ (relative-tol 0.95)
+ (absolute-tol 0.01))
+
+ ;; (say "pos-a " pos-a)
+ ;; (say "pos-b " pos-b)
+ ;; (say "dp " dp)
+ ;; (say "dp " dp)
+ ;; (say "rot-at " rot-at)
+ ;; (say "da " da)
+
+ (set! axis FACE-A-X)
+
+ (set! separation face-a.x)
+
+ (set! normal
+ (v*n rot-a.col-1
+ (if (> da.x 0.0) 1 -1)))
+
+ ;; (say "normal " normal)
+
+ (if (> face-a.y (+ (* relative-tol separation)
+ (* absolute-tol ha.y)))
+ (begin
+ ;; (say "********** BRANCH A **********")
+ ;; (say "da.y " da.y)
+ (set! axis FACE-A-Y)
+ (set! separation face-a.y)
+ (set! normal (v*n rot-a.col-2
+ (if (> da.y 0.0) 1 -1)))))
+
+ (if (> face-b.x (+ (* relative-tol separation)
+ (* absolute-tol hb.x)))
+ (begin
+ ;; (say "********** BRANCH B **********")
+ (set! axis FACE-B-X)
+ (set! separation face-b.x)
+ (set! normal (v*n rot-b.col-1
+ (if (> db.x 0.0) 1 -1)))))
+
+ (if (> face-b.y (+ (* relative-tol separation)
+ (* absolute-tol hb.y)))
+ (begin
+ ;; (say "********** BRANCH C **********")
+ (set! axis FACE-B-Y)
+ (set! separation face-b.y)
+ (set! normal (v*n rot-b.col-2
+ (if (> db.y 0.0) 1 -1)))))
+
+ (let (
+ (front-normal #f)
+ (side-normal #f)
+
+ ;; (incident-edge (make-vector 2))
+
+ (incident-edge
+ (vector
+ (make-clip-vertex (make-vec 0 0)
+ (make-feature-pair (create-edges) 0))
+ (make-clip-vertex (make-vec 0 0)
+ (make-feature-pair (create-edges) 0))))
+
+ ;; (incident-edge (vector (create-clip-vertex)
+ ;; (create-clip-vertex)))
+
+ (front #f)
+ (neg-side #f)
+ (pos-side #f)
+ (neg-edge #f)
+ (pos-edge #f)
+ (side #f)
+ )
+
+ ;; (say-expr incident-edge)
+
+ (case axis
+
+ ((FACE-A-X)
+
+ ;; (display "case FACE-A-X ")
+
+ (set! front-normal normal)
+ (set! front (+ (vec-dot pos-a front-normal) ha.x))
+ (set! side-normal rot-a.col-2)
+ (set! side (vec-dot pos-a side-normal))
+ (set! neg-side (+ (- side) ha.y))
+ (set! pos-side (+ side ha.y))
+ (set! neg-edge EDGE3)
+ (set! pos-edge EDGE1)
+
+ (compute-incident-edge
+ incident-edge hb pos-b rot-b front-normal)
+
+ )
+
+ ((FACE-A-Y)
+
+ ;; (display "case FACE-A-Y ")
+
+ (set! front-normal normal)
+ (set! front (+ (vec-dot pos-a front-normal) ha.y))
+ (set! side-normal rot-a.col-1)
+ (set! side (vec-dot pos-a side-normal))
+ (set! neg-side (+ (- side) ha.x))
+ (set! pos-side (+ side ha.x))
+ (set! neg-edge EDGE2)
+ (set! pos-edge EDGE4)
+
+ (compute-incident-edge
+ incident-edge hb pos-b rot-b front-normal))
+
+ ((FACE-B-X)
+
+ ;; (display "case FACE-B-X ")
+
+ (set! front-normal (vec::neg normal))
+ (set! front (+ (vec-dot pos-b front-normal) hb.x))
+ (set! side-normal rot-b.col-2)
+ (set! side (vec-dot pos-b side-normal))
+ (set! neg-side (+ (- side) hb.y))
+ (set! pos-side (+ side hb.y))
+ (set! neg-edge EDGE3)
+ (set! pos-edge EDGE1)
+ (compute-incident-edge
+ incident-edge ha pos-a rot-a front-normal))
+
+ ((FACE-B-Y)
+
+ ;; (display "case FACE-B-Y ")
+
+ (set! front-normal (vec::neg normal))
+ (set! front (+ (vec-dot pos-b front-normal) hb.y))
+ (set! side-normal rot-b.col-1)
+ (set! side (vec-dot pos-b side-normal))
+ (set! neg-side (+ (- side) hb.x))
+ (set! pos-side (+ side hb.x))
+ (set! neg-edge EDGE2)
+ (set! pos-edge EDGE4)
+ (compute-incident-edge
+ incident-edge ha pos-a rot-a front-normal))
+ )
+
+ ;; (say-expr incident-edge)
+
+ (let ((clip-points-1 (vector (create-clip-vertex)
+ (create-clip-vertex)))
+
+ (clip-points-2 (vector (create-clip-vertex)
+ (create-clip-vertex)))
+
+ )
+
+ ;; (say-expr clip-points-2)
+
+ ;; (say "incident-edge.0.v"
+ ;; (clip-vertex-v (vector-ref incident-edge 0))
+ ;; )
+ ;; (say "incident-edge.1.v"
+ ;; (clip-vertex-v (vector-ref incident-edge 1))
+ ;; )
+
+ (if (< (clip-segment-to-line clip-points-1
+ incident-edge
+ (vec::neg side-normal)
+ neg-side
+ neg-edge)
+ 2)
+
+ 0
+
+ (if (< (clip-segment-to-line clip-points-2
+ clip-points-1
+ side-normal
+ pos-side
+ pos-edge)
+ 2)
+
+ 0
+
+ (do ((num-contacts 0)
+ (i 0 (+ i 1)))
+ ((>= i 2)
+ ;; (say "num-contacts " num-contacts)
+ num-contacts)
+
+ ;; (say-expr clip-points-2)
+
+ (let ()
+
+ (is-vector contacts num-contacts)
+ (is-contact contacts.num-contacts)
+
+ (is-vector clip-points-2 i)
+ (is-clip-vertex clip-points-2.i)
+
+ (let ((separation
+ (- (vec-dot front-normal clip-points-2.i.v)
+ front)))
+
+ ;; (say "clip-points-1.0.v "
+ ;; (clip-vertex-v (vector-ref clip-points-1 0)))
+
+ ;; (say "clip-points-1.1.v "
+ ;; (clip-vertex-v (vector-ref clip-points-1 1)))
+
+ ;; (say "clip-points-2.0.v "
+ ;; (clip-vertex-v (vector-ref clip-points-2 0)))
+
+ ;; (say "clip-points-2.1.v "
+ ;; (clip-vertex-v (vector-ref clip-points-2 1)))
+
+
+ ;; (say "i " i)
+ ;; (say "front-normal " front-normal)
+ ;; (say "clip-points-2.i.v " clip-points-2.i.v)
+ ;; (say "separation " separation)
+
+ (if (<= separation 0)
+
+ (begin
+
+ (contacts.num-contacts.separation! separation)
+ (contacts.num-contacts.normal! normal)
+ (contacts.num-contacts.position!
+ (v- clip-points-2.i.v
+ (n*v separation front-normal)))
+
+ ;; (say-expr clip-points-2.i.fp)
+
+ (contacts.num-contacts.feature!
+ clip-points-2.i.fp)
+
+ (if (or (eq? axis FACE-B-X)
+ (eq? axis FACE-B-Y))
+ (flip contacts.num-contacts.feature))
+
+ (set! num-contacts
+ (+ num-contacts 1))))))))))))))))))
+
+ )
126 compute-incident-edge.sls
@@ -0,0 +1,126 @@
+
+(library (box2d-lite compute-incident-edge)
+
+ (export compute-incident-edge)
+
+ (import (rnrs)
+ (dharmalab misc is-vector)
+ (box2d-lite util say)
+ (box2d-lite util define-record-type)
+ (box2d-lite util math)
+ (box2d-lite vec)
+ (box2d-lite mat)
+ (box2d-lite clip-vertex)
+ (box2d-lite feature-pair)
+ (box2d-lite edge-numbers)
+ )
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (compute-incident-edge c h pos rot normal)
+
+ (define i 0)
+
+ (is-vector c i)
+ (is-clip-vertex c.i)
+ (is-vec c.i.v)
+ (is-feature-pair c.i.fp)
+ (is-edges c.i.fp.e)
+
+ (is-vec h)
+
+ (is-mat rot)
+
+ ;; (say "compute-incident-edge")
+
+ (let ((rot-t (rot.transpose)))
+
+ (let ((n (vec::neg (m*v rot-t normal))))
+
+ (let ((n-abs (vec::abs n)))
+
+ (is-vec n)
+ (is-vec n-abs)
+
+ ;; (say "n " n)
+
+ (if (> n-abs.x n-abs.y)
+
+ (if (> (sign n.x) 0.0)
+
+ (begin
+
+ ;; (say "********** BRANCH A B **********")
+
+ (set! i 0)
+
+ (c.i.v.set h.x (- h.y))
+ (c.i.fp.e.in-edge-2! EDGE3)
+ (c.i.fp.e.out-edge-2! EDGE4)
+
+ (set! i 1)
+
+ (c.i.v.set h.x h.y)
+ (c.i.fp.e.in-edge-2! EDGE4)
+ (c.i.fp.e.out-edge-2! EDGE1))
+
+ (begin
+
+ ;; (say "********** BRANCH A C **********")
+
+ (set! i 0)
+
+ (c.i.v.set (- h.x) h.y)
+ (c.i.fp.e.in-edge-2! EDGE1)
+ (c.i.fp.e.out-edge-2! EDGE2)
+
+ (set! i 1)
+
+ (c.i.v.set (- h.x) (- h.y))
+ (c.i.fp.e.in-edge-2! EDGE2)
+ (c.i.fp.e.out-edge-2! EDGE3)
+
+ ))
+
+ (if (> (sign n.y) 0.0)
+
+ (begin
+
+ ;; (say "********** BRANCH D E **********")
+
+ (set! i 0)
+
+ (c.i.v.set h.x h.y)
+ (c.i.fp.e.in-edge-2! EDGE4)
+ (c.i.fp.e.out-edge-2! EDGE1)
+
+ (set! i 1)
+
+ (c.i.v.set (- h.x) h.y)
+ (c.i.fp.e.in-edge-2! EDGE1)
+ (c.i.fp.e.out-edge-2! EDGE2))
+
+ (begin
+
+ ;; (say "********** BRANCH D F **********")
+
+ (set! i 0)
+
+ (c.i.v.set (- h.x) (- h.y))
+ (c.i.fp.e.in-edge-2! EDGE2)
+ (c.i.fp.e.out-edge-2! EDGE3)
+
+ (set! i 1)
+
+ (c.i.v.set h.x (- h.y))
+ (c.i.fp.e.in-edge-2! EDGE3)
+ (c.i.fp.e.out-edge-2! EDGE4)))))))
+
+ (set! i 0) (c.i.v! (v+ pos (m*v rot c.i.v)))
+ (set! i 1) (c.i.v! (v+ pos (m*v rot c.i.v)))
+
+ )
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ )
78 contact.sls
@@ -0,0 +1,78 @@
+
+(library (box2d-lite contact)
+
+ (export make-contact
+
+ is-contact
+ import-contact
+
+ contact-position
+ contact-normal
+ contact-r1
+ contact-r2
+ contact-separation
+ contact-pn
+ contact-pt
+ contact-pnb
+ contact-mass-normal
+ contact-mass-tangent
+ contact-bias
+ contact-feature
+
+ contact-position-set!
+ contact-normal-set!
+ contact-r1-set!
+ contact-r2-set!
+ contact-separation-set!
+ contact-pn-set!
+ contact-pt-set!
+ contact-pnb-set!
+ contact-mass-normal-set!
+ contact-mass-tangent-set!
+ contact-bias-set!
+ contact-feature-set!
+
+ create-contact
+
+ )
+
+ (import (rnrs)
+ (box2d-lite util define-record-type)
+ (box2d-lite vec)
+ (box2d-lite feature-pair)
+ )
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-record-type++ contact
+ is-contact
+ import-contact
+ (fields (mutable position)
+ (mutable normal)
+ (mutable r1)
+ (mutable r2)
+ (mutable separation)
+ (mutable pn)
+ (mutable pt)
+ (mutable pnb)
+ (mutable mass-normal)
+ (mutable mass-tangent)
+ (mutable bias)
+ (mutable feature))
+ (methods))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (create-contact)
+ (make-contact (make-vec 0 0)
+ (make-vec 0 0)
+ (make-vec 0 0)
+ (make-vec 0 0)
+ 0 0.0 0.0 0.0 0 0 0
+ (create-feature-pair)
+ ))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ )
+
189 demos/small-pyramid.sps
@@ -0,0 +1,189 @@
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(import (rnrs)
+ (srfi :27 random-bits)
+ (gl)
+ (glut)
+ (agave glamour misc)
+ (agave glamour window)
+ (box2d-lite util math)
+ (box2d-lite vec)
+ (box2d-lite mat)
+ (box2d-lite body)
+ (box2d-lite world))
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(initialize-glut)
+
+(window (size 800 800)
+ (title "Box2d Lite - Pyramid")
+ (reshape (width height)
+ (lambda (w h)
+ (glMatrixMode GL_PROJECTION)
+ (glLoadIdentity)
+ (glOrtho -20.0 20.0 -20.0 20.0 -1000.0 1000.0))))
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(random-source-randomize! default-random-source)
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(define bodies '())
+
+(define time-step (/ 1.0 60.0))
+
+(define world (make-world #f #f #f (make-vec 0.0 -10.0) 10))
+
+(is-world world)
+
+(define bomb #f)
+
+(is-body bomb)
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(define (launch-bomb)
+
+ (if (not bomb)
+
+ (begin
+
+ (set! bomb (create-body))
+
+ (bomb.set (make-vec 1.0 1.0) 50.0)
+
+ (bomb.friction! 0.2)
+
+ (world.add-body bomb)
+
+ (set! bodies (cons bomb bodies))
+
+ ))
+
+ (bomb.position! (make-vec (+ -15.0 (* (random-real) 30.0))
+ 15.0))
+
+ (bomb.rotation! (+ -1.5 (* (random-real) 3.0)))
+
+ (bomb.velocity! (n*v -1.5 bomb.position))
+
+ (bomb.angular-velocity! (+ -20.0 (* (random-real) 40.0))))
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(define draw-body-print-data? #f)
+
+(define (draw-body body)
+
+ (is-body body)
+
+ (let ((R (angle->mat body.rotation))
+ (x body.position)
+ (h (n*v 0.5 body.width)))
+
+ (is-vec h)
+
+ (let ((v1 (v+ x (m*v R (make-vec (- h.x) (- h.y)))))
+ (v2 (v+ x (m*v R (make-vec h.x (- h.y)))))
+ (v3 (v+ x (m*v R (make-vec h.x h.y ))))
+ (v4 (v+ x (m*v R (make-vec (- h.x) h.y )))))
+
+ (is-vec v1)
+ (is-vec v2)
+ (is-vec v3)
+ (is-vec v4)
+
+ (if (eq? body bomb)
+ (glColor3f 0.4 0.9 0.4)
+ (glColor3f 0.8 0.8 0.9))
+
+ (gl-begin GL_LINE_LOOP
+ (glVertex2d v1.x v1.y)
+ (glVertex2d v2.x v2.y)
+ (glVertex2d v3.x v3.y)
+ (glVertex2d v4.x v4.y)))))
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(world.clear)
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(let ((b (create-body)))
+
+ (is-body b)
+
+ (is-vec b.width)
+
+ (b.set (make-vec 100.0 20.0) FLT-MAX)
+
+ (b.friction! 0.2)
+
+ (b.position! (make-vec 0.0 (* -0.5 b.width.y)))
+
+ (b.rotation! 0.0)
+
+ (world.add-body b)
+
+ (set! bodies (cons b bodies)))
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(let ((b (create-body)))
+
+ (is-body b)
+
+ (define x (make-vec -6.0 0.75))
+
+ (define y #f)
+
+ (do ((i 0 (+ i 1)))
+ ((>= i 6))
+
+ (set! y x)
+
+ (do ((j i (+ j 1)))
+ ((>= j 6))
+
+ (b.set (make-vec 1.0 1.0) 10.0)
+
+ (b.friction! 0.2)
+
+ (b.position! y)
+
+ (world.add-body b)
+
+ (set! bodies (cons b bodies))
+
+ (set! y (v+ y (make-vec 1.125 0.0)))
+
+ (set! b (create-body))
+ )
+
+ (set! x (v+ x (make-vec 0.5625 2.0)))))
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(buffered-display-procedure
+ (lambda ()
+ (background 0.0)
+ (world.step time-step)
+ (for-each draw-body bodies)))
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(glutIdleFunc glutPostRedisplay)
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(glutKeyboardFunc
+ (lambda (key x y)
+ (case (integer->char key)
+ ((#\space) (launch-bomb)))))
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(glutMainLoop)
15 edge-numbers.sls
@@ -0,0 +1,15 @@
+
+(library (box2d-lite edge-numbers)
+
+ (export NO-EDGE EDGE1 EDGE2 EDGE3 EDGE4)
+
+ (import (rnrs))
+
+ (define NO-EDGE 'NO-EDGE)
+
+ (define EDGE1 'EDGE1)
+ (define EDGE2 'EDGE2)
+ (define EDGE3 'EDGE3)
+ (define EDGE4 'EDGE4)
+
+ )
106 feature-pair.sls
@@ -0,0 +1,106 @@
+
+(library (box2d-lite feature-pair)
+
+ (export make-edges
+
+ edges-in-edge-1
+ edges-out-edge-1
+ edges-in-edge-2
+ edges-out-edge-2
+
+ edges-in-edge-1-set!
+ edges-out-edge-1-set!
+ edges-in-edge-2-set!
+ edges-out-edge-2-set!
+
+ is-edges
+ import-edges
+
+ create-edges
+
+ edges-equal?
+
+ make-feature-pair
+
+ feature-pair-e
+ feature-pair-value
+
+ feature-pair-e-set!
+ feature-pair-value-set!
+
+ is-feature-pair
+ import-feature-pair
+
+ create-feature-pair
+
+ flip)
+
+ (import (rnrs)
+ (srfi :27 random-bits)
+ (box2d-lite util define-record-type)
+ (box2d-lite edge-numbers)
+ )
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-record-type++ edges
+ is-edges
+ import-edges
+ (fields (mutable in-edge-1)
+ (mutable out-edge-1)
+ (mutable in-edge-2)
+ (mutable out-edge-2))
+ (methods))
+
+ (define (create-edges)
+ (make-edges NO-EDGE NO-EDGE NO-EDGE NO-EDGE))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (edges-equal? a b)
+
+ (is-edges a)
+ (is-edges b)
+
+ (and (equal? a.in-edge-1 b.in-edge-1)
+ (equal? a.out-edge-1 b.out-edge-1)
+ (equal? a.in-edge-2 b.in-edge-2)
+ (equal? a.out-edge-2 b.out-edge-2)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-record-type++ feature-pair
+ is-feature-pair
+ import-feature-pair
+ (fields (mutable e)
+ (mutable value))
+ (methods))
+
+ ;; (define (create-feature-pair)
+ ;; (make-feature-pair (make-edges 0 0 0 0) 0))
+
+ (define (create-feature-pair)
+ (make-feature-pair (create-edges)
+ (random-integer 1000000000)
+ ))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (flip fp)
+
+ (is-feature-pair fp)
+ (is-edges fp.e)
+
+ (let ((tmp fp.e.in-edge-1))
+
+ (fp.e.in-edge-1! fp.e.in-edge-2)
+
+ (fp.e.in-edge-2! tmp))
+
+ (let ((tmp fp.e.out-edge-1))
+
+ (fp.e.out-edge-1! fp.e.out-edge-2)
+
+ (fp.e.out-edge-2! tmp)))
+
+ )
125 joint.sls
@@ -0,0 +1,125 @@
+
+(library (box2d-lite joint)
+
+ (export make-joint
+
+ joint::pre-step
+ joint::apply-impulse
+ )
+
+ (import (rnrs)
+ (box2d-lite util define-record-type)
+ (box2d-lite vec)
+ (box2d-lite mat)
+ (box2d-lite body)
+ (box2d-lite world-parameters)
+ )
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-record-type++ joint
+ is-joint
+ import-joint
+ (fields (mutable m)
+ (mutable local-anchor-1)
+ (mutable local-anchor-2)
+ (mutable r1)
+ (mutable r2)
+ (mutable bias)
+ (mutable p)
+ (mutable body-1)
+ (mutable body-2)
+ (mutable bias-factor)
+ (mutable softness))
+ (methods)
+ )
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (joint::pre-step j inv-dt)
+
+ (import-joint j)
+
+ (is-body body-1)
+ (is-body body-2)
+
+ (is-vec r1)
+ (is-vec r2)
+
+ (is-vec bias)
+ (is-vec p)
+
+ (r1! (m*v (angle->mat body-1.rotation) local-anchor-1))
+ (r2! (m*v (angle->mat body-2.rotation) local-anchor-2))
+
+ (m!
+ (mat::invert
+ (m+m
+ (mat-by-rows (+ body-1.inv-mass body-2.inv-mass) 0.0
+ 0.0 (+ body-1.inv-mass body-2.inv-mass))
+
+ (mat-by-rows (* body-1.inv-i r1.y r1.y ) (* body-1.inv-i r1.x r1.y -1)
+ (* body-1.inv-i r1.x r1.y -1) (* body-1.inv-i r1.x r1.x ))
+
+ (mat-by-rows (* body-2.inv-i r2.y r2.y ) (* body-2.inv-i r2.x r2.y -1)
+ (* body-2.inv-i r2.x r2.y -1) (* body-2.inv-i r2.x r2.x ))
+
+ (mat-by-rows softness 0
+ 0 softness))))
+
+ (if (position-correction)
+ (bias! (n*v (* -1 bias-factor inv-dt)
+ (v- (v+ body-2.position r2)
+ (v+ body-1.position r1))))
+ (bias.set 0.0 0.0))
+
+ (if (warm-starting)
+
+ (begin
+
+ (body-1.velocity! (v- body-1.velocity (n*v body-1.inv-mass p)))
+
+ (body-1.angular-velocity!
+ (- body-1.angular-velocity (* body-1.inv-i (vxv r1 p))))
+
+ (body-2.velocity! (v+ body-2.velocity (n*v body-2.inv-mass p)))
+
+ (body-2.angular-velocity!
+ (+ body-2.angular-velocity (* body-2.inv-i (vxv r2 p)))))
+
+ (p.set 0.0 0.0)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (joint::apply-impulse j)
+
+ (import-joint j)
+
+ (is-body body-1)
+ (is-body body-2)
+
+ (let ((dv (v- (v- (v+ body-2.velocity
+ (nxv body-2.angular-velocity
+ r2))
+ body-1.velocity)
+ (nxv body-1.angular-velocity r1))))
+
+ (let ((impulse (m*v m (v- (v- bias dv) (n*v softness p)))))
+
+ (body-1.velocity! (v- body-1.velocity (n*v body-1.inv-mass impulse)))
+
+ (body-1.angular-velocity!
+ (- body-1.angular-velocity (* body-1.inv-i (vxv r1 impulse))))
+
+ (body-2.velocity! (v+ body-1.velocity (n*v body-2.inv-mass impulse)))
+
+ (body-2.angular-velocity!
+ (+ body-2.angular-velocity (* body-2.inv-i (vxv r2 impulse))))
+
+ (p! (v+ p impulse)))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ )
+
+
147 mat.sls
@@ -0,0 +1,147 @@
+
+(library (box2d-lite mat)
+
+ (export make-mat
+
+ mat-col-1
+ mat-col-2
+
+ mat-col-1-set!
+ mat-col-2-set!
+
+ is-mat
+ import-mat
+
+ mat::abs
+ mat::transpose
+ mat::invert
+
+ m+
+ m*
+
+ m*v
+
+ mat-by-rows
+ m+m
+ angle->mat
+ )
+
+ (import (rnrs)
+
+ (box2d-lite util define-record-type)
+
+ (box2d-lite vec))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-record-type++ mat
+ is-mat
+ import-mat
+ (fields (mutable col-1)
+ (mutable col-2))
+ (methods (transpose mat::transpose)
+ (invert mat::invert)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (mat::abs m)
+ (is-mat m)
+ (make-mat (vec::abs m.col-1)
+ (vec::abs m.col-2)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (mat::transpose m)
+
+ (import-mat m)
+
+ (is-vec col-1)
+ (is-vec col-2)
+
+ (make-mat (make-vec col-1.x col-2.x)
+ (make-vec col-1.y col-2.y)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (mat::invert m)
+
+ (import-mat m)
+
+ (is-vec col-1)
+ (is-vec col-2)
+
+ (let ((a col-1.x) (b col-2.x) (c col-1.y) (d col-2.y))
+
+ (let ((det (/ 1.0 (- (* a d) (* b c)))))
+
+ (mat-by-rows (* det d ) (* det b -1)
+ (* det c -1) (* det a )))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (m+ a b)
+
+ (is-mat a)
+ (is-mat b)
+
+ (make-mat (v+ a.col-1 b.col-1)
+ (v+ a.col-2 b.col-2)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (m*v m v)
+
+ (is-mat m)
+ (is-vec v)
+
+ (is-vec m.col-1)
+ (is-vec m.col-2)
+
+ (make-vec (+ (* m.col-1.x v.x)
+ (* m.col-2.x v.y))
+ (+ (* m.col-1.y v.x)
+ (* m.col-2.y v.y))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (m* a b)
+
+ (is-mat a)
+ (is-mat b)
+
+ (make-mat (m*v a b.col-1)
+ (m*v a b.col-2)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-syntax mat-by-rows
+ (syntax-rules ()
+ ((_ (a b) (c d))
+ (make-mat (make-vec a c)
+ (make-vec b d)))
+ ((_ a b c d)
+ (mat-by-rows (a b)
+ (c d)))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-syntax m+m
+ (syntax-rules ()
+ ((m+m a b) (m+ a b))
+ ((m+m a b c ...)
+ (m+m (m+ a b) c ...))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (angle->mat angle)
+
+ (let ((c (cos angle))
+ (s (sin angle)))
+
+ (mat-by-rows c (- s)
+ s c)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ )
+
68 util/define-import-type-syntax.sls
@@ -0,0 +1,68 @@
+
+(library (box2d-lite util define-import-type-syntax)
+
+ (export define-imported-field-syntax
+ define-import-type-syntax)
+
+ (import (rnrs)
+ (dharmalab misc gen-id))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-syntax define-imported-get-field-syntax
+ (lambda (stx)
+ (syntax-case stx ()
+ ((_ var type field)
+ (with-syntax ((name (gen-id #'var #'field))
+ (getter (gen-id #'var #'type "-" #'field)))
+ #'(define-syntax name
+ (identifier-syntax
+ (getter var))))))))
+
+ (define-syntax define-imported-set-field-syntax
+ (lambda (stx)
+ (syntax-case stx ()
+ ((_ var type field)
+ (with-syntax ((name! (gen-id #'var #'field "!"))
+ (setter (gen-id #'var #'type "-" #'field "-set!")))
+ #'(define-syntax name!
+ (syntax-rules ()
+ ((name! val)
+ (setter var val)))))))))
+
+ (define-syntax define-imported-field-syntax
+ (syntax-rules ()
+ ((define-imported-field-syntax var type field)
+ (begin
+ (define-imported-get-field-syntax var type field)
+ (define-imported-set-field-syntax var type field)))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-syntax define-imported-record-method-syntax
+ (lambda (stx)
+ (syntax-case stx ()
+ ((_ var name proc)
+ (with-syntax ((met (gen-id #'var #'name)))
+ #'(define-syntax met
+ (syntax-rules ()
+ ((met arg (... ...))
+ (proc var arg (... ...))))))))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-syntax define-import-type-syntax
+ (syntax-rules (fields methods)
+ ((_ import-type type (fields field ...) (methods (name proc) ...))
+ (define-syntax import-type
+ (syntax-rules ()
+ ((import-type var)
+ (begin
+ (define-imported-field-syntax var type field)
+ ...
+ (define-imported-record-method-syntax var name proc)
+ ...)))))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ )
70 util/define-is-type-syntax.sls
@@ -0,0 +1,70 @@
+
+(library (box2d-lite util define-is-type-syntax)
+
+ (export define-field-syntax
+ define-is-type-syntax
+ )
+
+ (import (rnrs)
+ (dharmalab misc gen-id))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-syntax define-get-field-syntax
+ (lambda (stx)
+ (syntax-case stx ()
+ ((_ var type field)
+ (with-syntax ((var.field (gen-id #'var #'var "." #'field))
+ (getter (gen-id #'var #'type "-" #'field)))
+ #'(define-syntax var.field
+ (identifier-syntax
+ (getter var))))))))
+
+ (define-syntax define-set-field-syntax
+ (lambda (stx)
+ (syntax-case stx ()
+ ((_ var type field)
+ (with-syntax ((var.field! (gen-id #'var #'var "." #'field "!"))
+ (setter (gen-id #'var #'type "-" #'field "-set!")))
+ #'(define-syntax var.field!
+ (syntax-rules ()
+ ((var.field! val)
+ (setter var val)))))))))
+
+ (define-syntax define-field-syntax
+ (syntax-rules ()
+ ((define-field-syntax var type field)
+ (begin
+ (define-get-field-syntax var type field)
+ (define-set-field-syntax var type field)))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-syntax define-record-method-syntax
+ (lambda (stx)
+ (syntax-case stx ()
+ ((_ var name proc)
+ (with-syntax ((var.name (gen-id #'var #'var "." #'name)))
+ (syntax
+ (define-syntax var.name
+ (syntax-rules ()
+ ((var.name arg (... ...))
+ (proc var arg (... ...)))))))))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-syntax define-is-type-syntax
+ (syntax-rules (fields methods)
+ ((_ is-type type (fields field ...) (methods (name proc) ...))
+ (define-syntax is-type
+ (syntax-rules ()
+ ((is-type var)
+ (begin
+ (define-field-syntax var type field)
+ ...
+ (define-record-method-syntax var name proc)
+ ...)))))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ )
38 util/define-record-type.sls
@@ -0,0 +1,38 @@
+
+(library (box2d-lite util define-record-type)
+
+ (export define-record-type++)
+
+ (import (rnrs)
+ (box2d-lite util define-is-type-syntax)
+ (box2d-lite util define-import-type-syntax))
+
+ (define-syntax define-record-type++
+
+ (syntax-rules (fields methods)
+
+ ((define-record-type++ type
+ is-type
+ import-type
+ (fields (mutable field)
+ ...)
+ (methods (name proc)
+ ...))
+
+ (begin
+
+ (define-record-type type
+ (fields (mutable field)
+ ...))
+
+ (define-is-type-syntax is-type
+ type
+ (fields field ...)
+ (methods (name proc) ...))
+
+ (define-import-type-syntax import-type
+ type
+ (fields field ...)
+ (methods (name proc) ...))))))
+
+ )
18 util/math.sls
@@ -0,0 +1,18 @@
+
+(library (box2d-lite util math)
+
+ (export FLT-MAX pi sign clamp)
+
+ (import (rnrs))
+
+ (define FLT-MAX 3.40282e+38)
+
+ (define pi 3.14159265358979323846264)
+
+ (define (sign n)
+ (if (< n 0) -1 1))
+
+ (define (clamp n low high)
+ (max low (min n high)))
+
+ )
21 util/say.sls
@@ -0,0 +1,21 @@
+
+(library (box2d-lite util say)
+
+ (export say say-expr)
+
+ (import (rnrs))
+
+ (define (say . elts)
+ (for-each display elts)
+ (newline))
+
+ (define-syntax say-expr
+ (syntax-rules ()
+ ((say-expr expr)
+ (begin
+ (display 'expr)
+ (display " ")
+ (display expr)
+ (newline)))))
+
+ )
195 vec.sls
@@ -0,0 +1,195 @@
+
+(library (box2d-lite vec)
+
+ (export make-vec
+
+ vec-x
+ vec-y
+
+ vec-x-set!
+ vec-y-set!
+
+ is-vec
+ import-vec
+
+ vec::set
+ vec::length
+ vec::abs
+ vec::neg
+
+ v+ v- v* v/
+
+ v+n v*n
+
+ n+v
+ n-v
+ n*v
+
+ vec-dot
+
+ vxv
+
+ vxn
+
+ nxv
+
+ v-v
+
+ )
+
+ (import (rnrs)
+ (box2d-lite util define-record-type))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-record-type++ vec
+ is-vec
+ import-vec
+
+ (fields (mutable x)
+ (mutable y))
+
+ (methods (set vec::set)
+ (length vec::length)
+ (abs vec::abs)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (vec::set v new-x new-y)
+ (import-vec v)
+ (x! new-x)
+ (y! new-y))
+
+ (define (vec::length v)
+ (import-vec v)
+ (sqrt (+ (* x x) (* y y))))
+
+ (define (vec::abs v)
+ (is-vec v)
+ (make-vec (abs v.x) (abs v.y)))
+
+ (define (vec::neg v)
+ (is-vec v)
+ (make-vec (- v.x) (- v.y)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (v+ a b)
+
+ (is-vec a)
+ (is-vec b)
+
+ (make-vec (+ a.x b.x)
+ (+ a.y b.y)))
+
+ (define (v- a b)
+
+ (is-vec a)
+ (is-vec b)
+
+ (make-vec (- a.x b.x)
+ (- a.y b.y)))
+
+ (define (v* a b)
+
+ (is-vec a)
+ (is-vec b)
+
+ (make-vec (* a.x b.x)
+ (* a.y b.y)))
+
+ (define (v/ a b)
+
+ (is-vec a)
+ (is-vec b)
+
+ (make-vec (/ a.x b.x)
+ (/ a.y b.y)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (v+n v n)
+
+ (is-vec v)
+
+ (make-vec (+ v.x n)
+ (+ v.y n)))
+
+ (define (n*v n v)
+
+ (is-vec v)
+
+ (make-vec (* n v.x)
+ (* n v.y)))
+
+ (define (v*n v n)
+
+ (is-vec v)
+
+ (make-vec (* n v.x)
+ (* n v.y)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (n+v n v)
+
+ (is-vec v)
+
+ (make-vec (+ n v.x)
+ (+ n v.y)))
+
+ (define (n-v n v)
+
+ (is-vec v)
+
+ (make-vec (- n v.x)
+ (- n v.y)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+
+
+
+
+ (define (vec-dot a b)
+
+ (is-vec a)
+ (is-vec b)
+
+ (+ (* a.x b.x)
+ (* a.y b.y)))
+
+ (define (vxv a b)
+
+ (is-vec a)
+ (is-vec b)
+
+ (- (* a.x b.y)
+ (* a.y b.x)))
+
+ (define (vxn v n)
+
+ (is-vec v)
+
+ (make-vec (* n v.y)
+ (* (- n) v.x)))
+
+ (define (nxv n v)
+
+ (is-vec v)
+
+ (make-vec (* (- n) v.y)
+ (* n v.x)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-syntax v-v
+ (syntax-rules ()
+ ((v-v a b)
+ (v- a b))
+ ((v-v a b c ...)
+ (v-v (v- a b) c ...))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ )
19 world-parameters.sls
@@ -0,0 +1,19 @@
+
+(library (box2d-lite world-parameters)
+
+ (export accumulate-impulses
+ warm-starting
+ position-correction)
+
+ (import (rnrs)
+ (srfi :39 parameters))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define accumulate-impulses (make-parameter #t))
+ (define warm-starting (make-parameter #t))
+ (define position-correction (make-parameter #t))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ )
210 world.sls
@@ -0,0 +1,210 @@
+
+(library (box2d-lite world)
+
+ (export make-world
+
+ is-world
+ import-world
+
+ world::step
+
+ ;; for testing
+
+ world::broad-phase
+
+ world-bodies
+ world-joints
+ world-arbiters
+ world-gravity
+ world-iterations
+ )
+
+ (import (except (rnrs) remove)
+ (only (srfi :1 lists) remove)
+ (box2d-lite util define-record-type)
+ (box2d-lite util say)
+ (box2d-lite vec)
+ (box2d-lite body)
+ (box2d-lite joint)
+ (box2d-lite arbiter)
+
+ ;; for testing
+ (box2d-lite contact)
+ (box2d-lite feature-pair)
+
+ )
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (say-vec v) (say (vec-x v) " " (vec-y v)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define-record-type++ world
+ is-world
+ import-world
+ (fields (mutable bodies)
+ (mutable joints)
+ (mutable arbiters)
+ (mutable gravity)
+ (mutable iterations))
+ (methods (add-body world::add-body)
+ (clear world::clear)
+ (step world::step)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ ;; (define (world::add-body w body)
+ ;; (is-world w)
+ ;; (w.bodies! (cons body w.bodies)))
+
+ (define (world::add-body w body)
+ (is-world w)
+ (w.bodies! (append w.bodies (list body))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (world::clear w)
+ (is-world w)
+ (w.bodies! '())
+ (w.joints! '())
+ (w.arbiters! '()))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (world::broad-phase w)
+
+ (import-world w)
+
+ (do ((bodies bodies (cdr bodies)))
+ ((null? bodies))
+
+ (let ((bi (car bodies)))
+
+ (is-body bi)
+
+ (do ((bodies (cdr bodies) (cdr bodies)))
+ ((null? bodies))
+
+ (let ((bj (car bodies)))
+
+ (is-body bj)
+
+ (if (and (= bi.inv-mass 0.0) (= bj.inv-mass 0.0))
+
+ #t
+
+ (let ((new-arb (create-arbiter bi bj)))
+
+ (is-arbiter new-arb)
+
+ (if (> new-arb.num-contacts 0)
+
+ (let ((arbiter (find
+
+ (lambda (arbiter)
+ (is-arbiter arbiter)
+ (or (and (eq? bi arbiter.body-1)
+ (eq? bj arbiter.body-2))
+ (and (eq? bi arbiter.body-2)
+ (eq? bj arbiter.body-1)))
+ )
+
+ arbiters)))
+
+ (is-arbiter arbiter)
+
+ (if arbiter
+ (arbiter.update new-arb.contacts
+ new-arb.num-contacts)
+
+ (arbiters! (append arbiters (list new-arb)))
+
+ ))
+
+ (begin
+
+ (arbiters!
+ (remove
+ (lambda (arbiter)
+ (is-arbiter arbiter)
+ (or (and (eq? bi arbiter.body-1)
+ (eq? bj arbiter.body-2))
+ (and (eq? bi arbiter.body-2)
+ (eq? bj arbiter.body-1)))
+ )
+ arbiters))
+
+ )
+
+ ))))))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (world::step w dt)
+
+ (import-world w)
+
+ ;; (say "world::step "
+ ;; "bodies: " (length (world-bodies w)) " "
+ ;; "joints: " (length (world-joints w)) " "
+ ;; "arbiters: " (length (world-arbiters w)))
+
+ ;; (if (>= (length (world-bodies w)) 2)
+ ;; (say (body-velocity (list-ref (world-bodies w) 1))
+ ;; " "
+ ;; (body-angular-velocity (list-ref (world-bodies w) 1))))
+
+ ;; (if (>= (length (world-bodies w)) 2)
+ ;; (say
+
+ ;; ;; (vec-x (body-velocity (list-ref (world-bodies w) 2)))
+ ;; ;; " "
+ ;; ;; (vec-y (body-velocity (list-ref (world-bodies w) 2)))
+
+ ;; ;; (body-torque (list-ref (world-bodies w) 1))
+
+ ;; ))
+
+ (let ((inv-dt (if (> dt 0.0) (/ 1.0 dt) 0.0)))
+
+ (world::broad-phase w)
+
+ (for-each
+ (lambda (b)
+ (is-body b)
+ (if (= b.inv-mass 0.0)
+ #t
+ (begin
+
+ (b.velocity!
+ (v+ b.velocity (n*v dt (v+ gravity (n*v b.inv-mass b.force)))))
+ (b.angular-velocity!
+ (+ b.angular-velocity (* dt b.inv-i b.torque)))
+
+
+
+ )))
+ bodies)
+
+ (for-each (lambda (arbiter) (arbiter::pre-step arbiter inv-dt)) arbiters)
+ (for-each (lambda (joint) (joint::pre-step joint inv-dt)) joints)
+
+ (do ((i 0 (+ i 1)))
+ ((>= i iterations))
+ (for-each arbiter::apply-impulse arbiters)
+ (for-each joint::apply-impulse joints))
+
+ (for-each
+ (lambda (b)
+ (is-body b)
+ (b.position! (v+ b.position (n*v dt b.velocity)))
+ (b.rotation! (+ b.rotation (* dt b.angular-velocity)))
+ (vec::set b.force 0.0 0.0)
+ (b.torque! 0.0))
+ bodies)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ )
+
Please sign in to comment.
Something went wrong with that request. Please try again.