Permalink
Browse files

Hacking on the joints code. First joints based demo checked in (simpl…

…e-pendulum)
  • Loading branch information...
1 parent 8538907 commit cf03ae6efd81a5c06c4300a609da42c08e294da5 @dharmatech committed Dec 25, 2009
Showing with 297 additions and 12 deletions.
  1. +223 −0 demos/simple-pendulum.sps
  2. +63 −9 joint.sls
  3. +11 −3 world.sls
View
@@ -0,0 +1,223 @@
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(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 joint)
+ (box2d-lite world))
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(initialize-glut)
+
+(window (size 800 800)
+ (title "Box2d Lite - Simple Pendulum")
+ (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 joints '())
+
+(define time-step 0.001)
+
+(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)))))
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(define (draw-joint joint)
+
+ (is-joint joint)
+
+ (let ((b1 joint.body-1)
+ (b2 joint.body-2))
+
+ (is-body b1)
+ (is-body b2)
+
+ (let ((x1 b1.position)
+ (x2 b2.position))
+
+ (is-vec x1)
+ (is-vec x2)
+
+ (let ((p1 (v+ x1 (m*v (angle->mat b1.rotation) joint.local-anchor-1)))
+ (p2 (v+ x2 (m*v (angle->mat b2.rotation) joint.local-anchor-2))))
+
+ (is-vec p1)
+ (is-vec p2)
+
+ (glColor3f 0.5 0.5 0.8)
+
+ (gl-begin GL_LINES
+
+ (glVertex2d x1.x x1.y)
+ (glVertex2d p1.x p1.y)
+ (glVertex2d x2.x x2.y)
+ (glVertex2d p2.x p2.y))))))
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(world.clear)
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(define b1 (create-body))
+
+(is-body b1)
+
+(b1.set (make-vec 100.0 20.0) FLT-MAX)
+
+(b1.friction! 0.2)
+
+(b1.position! (make-vec 0.0 -10.0))
+
+(b1.rotation! 0.0)
+
+(world.add-body b1)
+
+(set! bodies (append bodies (list b1)))
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(define b2 (create-body))
+
+(is-body b2)
+
+(b2.set (make-vec 1.0 1.0) 100.0)
+
+(b2.friction! 0.2)
+
+(b2.position! (make-vec 9.0 11.0))
+
+(b2.rotation! 0.0)
+
+(world.add-body b2)
+
+(set! bodies (append bodies (list b2)))
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(let ((j (create-joint)))
+
+ (is-joint j)
+
+ (j.set b1 b2 (make-vec 0.0 11.0))
+
+ (world.add-joint j)
+
+ (set! joints (append joints (list j))))
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(buffered-display-procedure
+ (lambda ()
+ (background 0.0)
+ (world.step time-step)
+ (for-each draw-body bodies)
+ (for-each draw-joint joints)))
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(glutIdleFunc glutPostRedisplay)
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(glutKeyboardFunc
+ (lambda (key x y)
+ (case (integer->char key)
+ ((#\space) (launch-bomb)))))
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(display "Press <space> to throw the bomb\n")
+
+;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+(glutMainLoop)
View
@@ -1,25 +1,39 @@
(library (box2d-lite joint)
- (export make-joint
-
+ (export make-joint is-joint import-joint
+
+ joint-m joint-m-set!
+ joint-local-anchor-1 joint-local-anchor-1-set!
+ joint-local-anchor-2 joint-local-anchor-2-set!
+ joint-r1 joint-r1-set!
+ joint-r2 joint-r2-set!
+ joint-bias joint-bias-set!
+ joint-p joint-p-set!
+ joint-body-1 joint-body-1-set!
+ joint-body-2 joint-body-2-set!
+ joint-bias-factor joint-bias-factor-set!
+ joint-softness joint-softness-set!
+
+ create-joint
+
+ joint::set
joint::pre-step
- joint::apply-impulse
- )
+ joint::apply-impulse)
(import (rnrs)
(box2d-lite util define-record-type)
(box2d-lite vec)
(box2d-lite mat)
(box2d-lite body)
- (box2d-lite world-parameters)
- )
+ (box2d-lite world-parameters))
;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(define-record-type++ joint
is-joint
import-joint
+
(fields (mutable m)
(mutable local-anchor-1)
(mutable local-anchor-2)
@@ -31,9 +45,49 @@
(mutable body-2)
(mutable bias-factor)
(mutable softness))
- (methods)
- )
+
+ (methods (set joint::set)
+ (pre-step joint::pre-step)
+ (apply-impulse joint::apply-impulse)))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (create-joint)
+ (let ((j (make-joint #f #f #f #f #f #f #f #f #f #f #f)))
+ (is-joint j)
+ (j.p! (make-vec 0.0 0.0))
+ (j.bias-factor! 0.2)
+ (j.softness! 0.0)
+ j))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
+ (define (joint::set j b1 b2 anchor)
+
+ (import-joint j)
+
+ (is-body body-1)
+ (is-body body-2)
+
+ (body-1! b1)
+ (body-2! b2)
+
+ (local-anchor-1!
+ (m*v (mat::transpose (angle->mat body-1.rotation))
+ (v- anchor body-1.position)))
+
+ (local-anchor-2!
+ (m*v (mat::transpose (angle->mat body-2.rotation))
+ (v- anchor body-2.position)))
+
+ (p! (make-vec 0.0 0.0))
+
+ (softness! 0.0)
+
+ (bias-factor! 0.2)
+ j)
+
;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(define (joint::pre-step j inv-dt)
@@ -111,7 +165,7 @@
(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.velocity! (v+ body-2.velocity (n*v body-2.inv-mass impulse)))
(body-2.angular-velocity!
(+ body-2.angular-velocity (* body-2.inv-i (vxv r2 impulse))))
View
@@ -48,9 +48,11 @@
(mutable arbiters)
(mutable gravity)
(mutable iterations))
- (methods (add-body world::add-body)
- (clear world::clear)
- (step world::step)))
+
+ (methods (add-body world::add-body)
+ (add-joint world::add-joint)
+ (clear world::clear)
+ (step world::step)))
;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
@@ -64,6 +66,12 @@
;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+ (define (world::add-joint w joint)
+ (is-world w)
+ (w.joints! (append w.joints (list joint))))
+
+ ;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
+
(define (world::clear w)
(is-world w)
(w.bodies! '())

0 comments on commit cf03ae6

Please sign in to comment.