├── README ├── .mailmap ├── src ├── constraints │ ├── spring.lisp │ ├── constraints.lisp │ ├── breakable-joint.lisp │ ├── util.lisp │ ├── simple-motor.lisp │ ├── damped-rotary-spring.lisp │ ├── gear-joint.lisp │ ├── pivot-joint.lisp │ ├── ratchet-joint.lisp │ ├── damped-spring.lisp │ ├── rotary-limit-joint.lisp │ ├── slide-joint.lisp │ ├── pin-joint.lisp │ └── groove-joint.lisp ├── convenience.lisp ├── squirl.lisp ├── bounding-box.lisp ├── contact.lisp ├── package.lisp ├── hash-set.lisp ├── utils.lisp ├── poly-shape.lisp ├── vec.lisp ├── world-hash.lisp ├── arbiter.lisp ├── body.lisp ├── shape.lisp ├── world.lisp └── collision.lisp ├── .gitignore ├── COPYING ├── demo ├── pyramid-stack.lisp ├── plink.lisp ├── tumble.lisp ├── planet.lisp ├── pyramid.lisp ├── logo-smash.lisp ├── theo-jansen.lisp ├── pump.lisp ├── springies.lisp ├── demo.lisp ├── draw-world.lisp └── squirl-demo.lisp └── squirl.asd /README: -------------------------------------------------------------------------------- 1 | Native Lisp port of the Chipmunk physics library. 2 | -------------------------------------------------------------------------------- /.mailmap: -------------------------------------------------------------------------------- 1 | # Mail Map for Git 2 | 3 | Kat Marchán 4 | Adlai Chandrasekhar 5 | Benjamin Saunders Ralith 6 | Michael Compton 7 | -------------------------------------------------------------------------------- /src/constraints/spring.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defstruct (spring (:include constraint)) 5 | stiffness dt) 6 | 7 | (defmethod get-impulse ((spring spring)) 8 | 0d0) 9 | 10 | (defgeneric spring-torque (spring relative-angle)) 11 | (defgeneric spring-force (spring distance)) 12 | -------------------------------------------------------------------------------- /src/constraints/constraints.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defvar *constraint-bias-coefficient* 0.1) 5 | 6 | (defgeneric pre-step (constraint dt dt-inverse)) 7 | (defgeneric apply-impulse (constraint)) 8 | (defgeneric get-impulse (constraint)) 9 | 10 | (defstruct constraint 11 | body-a body-b 12 | (max-force most-positive-double-float) 13 | (bias-coefficient *constraint-bias-coefficient*) 14 | (max-bias most-positive-double-float) 15 | data) 16 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # CCL uses a different extension for each architecture/OS combination 2 | *.dfsl # DarwinPPC32 3 | *.pfsl # LinuxPPC32 4 | *.d64fsl # DarwinPPC64 5 | *.p64fsl # LinuxPPC64 6 | *.lx64fsl # LinuxX8664 7 | *.lx32fsl # LinuxX8632 8 | *.dx64fsl # DarwinX8664 9 | *.dx32fsl # DarwinX8632 10 | *.fx64fsl # FreeBSDX8664 11 | *.fx32fsl # FreeBSDX8632 12 | *.sx64fsl # SolarisX64 13 | *.sx32fsl # SolarisX86 14 | *.wx64fsl # Win64 15 | *.wx32fsl # Win32 16 | 17 | # SBCL (I think Allegro uses this too) 18 | *.fasl 19 | 20 | # CLISP 21 | *.fas 22 | 23 | # ECL 24 | *.o 25 | 26 | # Editor backups 27 | *~ 28 | -------------------------------------------------------------------------------- /src/convenience.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defun make-rectangle (width height &key (restitution 0d0) (friction 0d0) (offset +zero-vector+)) 5 | (let* ((width/2 (/ width 2)) 6 | (height/2 (/ height 2)) 7 | (verts (list (vec (- width/2) height/2) 8 | (vec width/2 height/2) 9 | (vec width/2 (- height/2)) 10 | (vec (- width/2) (- height/2))))) 11 | (make-poly verts :restitution restitution :friction friction :offset offset))) 12 | 13 | (defun make-circle-body () 14 | ;; todo 15 | ) 16 | (defun make-segment-body () 17 | ;; todo 18 | ) 19 | (defun make-poly-body () 20 | ;; todo 21 | ) 22 | (defun make-rectangle-body () 23 | ;; todo 24 | ) -------------------------------------------------------------------------------- /src/squirl.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defun moment-of-inertia-for-circle (mass inner-diameter outer-diameter &optional (offset +zero-vector+)) 5 | "Calculate the moment of inertia for a circle. 6 | A solid circle has an inner diameter of 0." 7 | (+ (* mass 1/2 (+ (expt inner-diameter 2) (expt outer-diameter 2))) 8 | (* mass (vec-length-sq offset)))) 9 | 10 | (defun moment-of-inertia-for-segment (mass point-a point-b) 11 | "Calculate the moment of inertia for a line segment connecting POINT-A to POINT-B." 12 | (let ((length (vec-length (vec- point-b point-a)))) 13 | (+ (* mass length (/ length 12)) 14 | (* mass (vec-length-sq (vec* (vec+ point-a point-b) 0.5d0)))))) 15 | 16 | (defun moment-of-inertia-for-poly (m verts &optional (offset +zero-vector+)) 17 | "Calculate the moment of inertia for a solid convex polygon." 18 | (flet ((transform-vertex (vertex) (vec+ vertex offset))) 19 | (let ((vertices (coerce verts 'list))) ; I would dx, but shitux. 20 | (loop 21 | for vertex in vertices 22 | for v1 = (transform-vertex vertex) 23 | and v2 = (transform-vertex (car (last vertices))) then v1 24 | for a = (vec-cross v2 v1) sum a into sum2 25 | sum (* a (+ (vec. v1 v1) (vec. v1 v2) (vec. v2 v2))) into sum1 26 | finally (return (/ (* m sum1) (* 6 sum2))))))) 27 | -------------------------------------------------------------------------------- /COPYING: -------------------------------------------------------------------------------- 1 | SquirL is Copyright © 2009 Kat Marchán, Adlai Chadrasekhar, and Benjamin Saunders 2 | 3 | The original C source code for Chipmunk, which SquirL is directly based on, has the following 4 | copyright notice: 5 | 6 | Copyright (c) 2007 Scott Lembcke and Howling Moon Software 7 | 8 | License for SquirL, as well as the original Chipmunk source code: 9 | 10 | Permission is hereby granted, free of charge, to any person obtaining a copy 11 | of this software and associated documentation files (the "Software"), to deal 12 | in the Software without restriction, including without limitation the rights 13 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 14 | copies of the Software, and to permit persons to whom the Software is 15 | furnished to do so, subject to the following conditions: 16 | 17 | The above copyright notice and this permission notice shall be included in 18 | all copies or substantial portions of the Software. 19 | 20 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 21 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 22 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 23 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 24 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 25 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 26 | SOFTWARE. 27 | -------------------------------------------------------------------------------- /src/constraints/breakable-joint.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | ;; What we have here is a textbook case of a proxy pattern. 5 | ;; Instead of creating an is-a relationship (which in this case 6 | ;; would involve multiple inheritance), we instead wrap any given 7 | ;; joint with this breakable one, which adds the breaking behavior. 8 | ;; As such, we forward most of the behavior over to the delegate, but 9 | ;; we pass -this- object around. 10 | (defstruct (breakable-joint (:include constraint) 11 | (:constructor make-breakable-joint (delegate space))) 12 | delegate space (last-dt-inverse 0d0)) 13 | 14 | (defmethod pre-step ((joint breakable-joint) dt dt-inverse) 15 | (let ((delegate (breakable-joint-delegate joint))) 16 | (if (>= (* (get-impulse delegate) (breakable-joint-last-dt-inverse joint)) 17 | (breakable-joint-max-force joint)) 18 | ;; remove the breakable joint from the space... 19 | (world-remove-constraint (breakable-joint-space joint) joint) 20 | ;; otherwise, call pre-step on its delegate. 21 | (prog1 (pre-step delegate dt dt-inverse) 22 | (setf (breakable-joint-last-dt-inverse joint) dt-inverse))))) 23 | 24 | (defmethod apply-impulse ((joint breakable-joint)) 25 | (apply-impulse (breakable-joint-delegate joint))) 26 | 27 | (defmethod get-impulse ((joint breakable-joint)) 28 | (get-impulse (breakable-joint-delegate joint))) 29 | -------------------------------------------------------------------------------- /src/bounding-box.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (declaim (inline make-bbox)) 5 | (defstruct (bbox (:constructor make-bbox (left bottom right top))) 6 | "Bounding box used to simplify collision detection" 7 | (left (assert nil) :type double-float) 8 | (bottom (assert nil) :type double-float) 9 | (right (assert nil) :type double-float) 10 | (top (assert nil) :type double-float)) 11 | 12 | (defun bbox-intersects-p (a b) 13 | "Tests whether `bbox' A and B intersect" 14 | (and (<= (bbox-left a) (bbox-right b)) 15 | (<= (bbox-left b) (bbox-right a)) 16 | (<= (bbox-bottom a) (bbox-top b)) 17 | (<= (bbox-bottom b) (bbox-top a)))) 18 | 19 | (defun contains-bbox-p (bb other) 20 | "Tests whether `bbox' BB entirely contains the OTHER one" 21 | (and (< (bbox-left bb) (bbox-left other)) 22 | (> (bbox-right bb) (bbox-right other)) 23 | (< (bbox-bottom bb) (bbox-bottom other)) 24 | (> (bbox-top bb) (bbox-top other)))) 25 | 26 | (defun bbox-containts-vec-p (bb vec) 27 | "Tests whether `vec' VEC is entirely contained by `bbox' BB" 28 | (with-vec vec 29 | (and (< (bbox-left bb) vec.x) 30 | (> (bbox-right bb) vec.x) 31 | (< (bbox-bottom bb) vec.y) 32 | (> (bbox-top bb) vec.y)))) 33 | 34 | (defun bbox-clamp-vec (bb vec) 35 | "Clamps the vector to lie within the bbox" 36 | (vec (min (max (bbox-left bb) (vec-x vec)) (bbox-right bb)) 37 | (min (max (bbox-bottom bb) (vec-y vec)) (bbox-top bb)))) 38 | 39 | (defun bbox-wrap-vec (bb vec) 40 | "Wrap a vector to a bbox." 41 | (vec (+ (bbox-left bb) 42 | (mod (- (vec-x vec) (bbox-left bb)) 43 | (abs (- (bbox-right bb) (bbox-left bb))))) 44 | (+ (bbox-bottom bb) 45 | (mod (- (vec-y vec) (bbox-bottom bb)) 46 | (abs (- (bbox-top bb) (bbox-bottom bb))))))) 47 | -------------------------------------------------------------------------------- /src/constraints/util.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defun apply-impulses (body1 body2 r1 r2 j) 5 | (body-apply-impulse body1 (vec- j) r1) 6 | (body-apply-impulse body2 j r2) 7 | (values)) 8 | 9 | (defun k-tensor (body1 body2 r1 r2) 10 | ;; calculate mass matrix 11 | ;; C sources say: "If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross... 12 | (let* ((mass-sum (+ (body-inverse-mass body1) 13 | (body-inverse-mass body2))) 14 | 15 | ;; Start with I*mass-sum 16 | (k11 mass-sum) (k12 0d0) 17 | (k21 0d0) (k22 mass-sum) 18 | 19 | ;; influence from r1 20 | (b1-inverse-inertia (body-inverse-inertia body1)) 21 | (r1xsq (* (vec-x r1) (vec-x r1) b1-inverse-inertia)) 22 | (r1ysq (* (vec-y r1) (vec-y r1) b1-inverse-inertia)) 23 | (r1nxy (- (* (vec-x r1) (vec-y r1) b1-inverse-inertia))) 24 | 25 | ;; influence from r2 26 | (b2-inverse-inertia (body-inverse-inertia body2)) 27 | (r2xsq (* (vec-x r2) (vec-x r2) b2-inverse-inertia)) 28 | (r2ysq (* (vec-y r2) (vec-y r2) b2-inverse-inertia)) 29 | (r2nxy (- (* (vec-x r2) (vec-y r2) b2-inverse-inertia)))) 30 | ;; apply influence from r1 31 | (incf k11 r1ysq) (incf k12 r1nxy) 32 | (incf k21 r1nxy) (incf k22 r1xsq) 33 | 34 | ;; apply influence from r2 35 | (incf k11 r2ysq) (incf k12 r2nxy) 36 | (incf k21 r2nxy) (incf k22 r2xsq) 37 | 38 | ;; invert 39 | (let ((det-inv (/ (- (* k11 k22) (* k12 k21))))) 40 | 41 | ;; and we're done. 42 | (values (vec (* k22 det-inv) (- (* k12 det-inv))) 43 | (vec (- (* k21 det-inv)) (* k11 det-inv)))))) 44 | 45 | (defun mult-k (vr k1 k2) 46 | (vec (vec. vr k1) (vec. vr k2))) 47 | 48 | (defun impulse-max (constraint dt) 49 | (* (constraint-max-force constraint) dt)) 50 | -------------------------------------------------------------------------------- /demo/pyramid-stack.lisp: -------------------------------------------------------------------------------- 1 | (in-package :squirl-demo) 2 | 3 | (defclass pyramid-stack (demo) 4 | ((static-body :accessor static-body)) 5 | (:default-initargs :name "Pyramid Stack")) 6 | 7 | (defmethod init-demo ((demo pyramid-stack)) 8 | (reset-shape-id-counter) 9 | (setf (world demo) (make-world :iterations 20 :gravity (vec 0 -250))) 10 | ;; Create segments around the edge of the screen 11 | (setf (static-body demo) 12 | (world-add-body (world demo) 13 | (make-body :actor :not-grabbable 14 | :shapes (list (make-segment (vec -320 -240) (vec -320 240) 15 | :restitution 1 :friction 1) 16 | (make-segment (vec 320 -240) (vec 320 240) 17 | :restitution 1 :friction 1) 18 | (make-segment (vec -320 -240) (vec 320 -240) 19 | :restitution 1 :friction 1))))) 20 | (resize-world-active-hash (world demo) 40 1000) 21 | (resize-world-static-hash (world demo) 40 1000) 22 | (let ((verts (list (vec -15 -15) 23 | (vec -15 15) 24 | (vec 15 15) 25 | (vec 15 -15)))) 26 | ;; add lots of boxen 27 | (loop for i below 14 do 28 | (loop for j upto i do 29 | (world-add-body 30 | (world demo) 31 | (make-body :mass 1 :position (vec (- (* j 32) (* i 16)) 32 | (- 300 (* i 32))) 33 | :shapes (list (make-poly verts :friction 0.8)))))) 34 | ;; add ball to make things interesting 35 | (let ((radius 15)) 36 | (world-add-body (world demo) (make-body :mass 10 :position (vec 0 (+ -240 radius)) 37 | :shapes (list (make-circle radius :friction 0.9))))) 38 | (world demo))) 39 | 40 | (pushnew 'pyramid-stack *demos*) -------------------------------------------------------------------------------- /src/contact.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | ;;; SBCL chokes when this constructor is inlined 5 | #-sbcl (declaim (inline make-contact)) 6 | (defstruct (contact (:constructor make-contact (point normal distance &optional hash))) 7 | ;; Contact point and normal 8 | (point +zero-vector+ :type vec) 9 | (normal +zero-vector+ :type vec) 10 | ;; Penetration distance 11 | (distance 0d0 :type double-float) 12 | ;; Calculated by arbiter-prestep 13 | (r1 +zero-vector+ :type vec) 14 | (r2 +zero-vector+ :type vec) 15 | (normal-mass 0d0 :type double-float) 16 | (tangent-mass 0d0 :type double-float) 17 | (bounce 0d0 :type double-float) 18 | ;; Persistant contact information 19 | (accumulated-normal-impulse 0d0 :type double-float) 20 | (accumulated-frictional-impulse 0d0 :type double-float) 21 | (impulse-bias 0d0 :type double-float) 22 | (bias 0d0 :type double-float) 23 | ;; Hash value used as a (mostly) unique ID 24 | (hash 0 :type fixnum)) 25 | 26 | (defun contacts-sum-impulses (&rest contacts) 27 | (reduce #'vec+ contacts :initial-value +zero-vector+ 28 | :key (fun (vec* (contact-normal _) 29 | (contact-accumulated-normal-impulse _))))) 30 | 31 | (defun contact-impulse-with-friction (contact) 32 | (vec-rotate (contact-normal contact) 33 | (vec (contact-accumulated-normal-impulse contact) 34 | (contact-accumulated-frictional-impulse contact)))) 35 | 36 | (defun contacts-sum-impulses-with-friction (&rest contacts) 37 | (reduce #'vec+ contacts :initial-value +zero-vector+ 38 | :key #'contact-impulse-with-friction)) 39 | 40 | (defun contacts-estimate-crushing-impulse (&rest contacts) 41 | (loop for contact in contacts 42 | for impulse = (contact-impulse-with-friction contact) 43 | for vec-sum = +zero-vector+ then (vec+ vec-sum impulse) 44 | sum (vec-length impulse) into scalar-sum 45 | finally (return (- 1 (/ (vec-length vec-sum) scalar-sum))))) 46 | 47 | -------------------------------------------------------------------------------- /src/constraints/simple-motor.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defstruct (simple-motor (:include constraint) 5 | (:constructor 6 | make-simple-motor 7 | (body-a body-b rate))) 8 | (rate 0d0) 9 | (i-sum 0d0) 10 | (j-acc 0d0) 11 | (j-max 0d0)) 12 | 13 | (defmethod pre-step ((motor simple-motor) dt dt-inv) 14 | (declare (ignore dt-inv)) 15 | (with-accessors ((body-a simple-motor-body-a) 16 | (body-b simple-motor-body-b) 17 | (i-sum simple-motor-i-sum) 18 | (j-max simple-motor-j-max) 19 | (j-acc simple-motor-j-acc)) motor 20 | ;; calculate moment of inertia coefficient. 21 | (setf i-sum (/ 1d0 (+ (body-inverse-inertia body-b) (body-inverse-inertia body-b)))) 22 | ;; compute max impulse 23 | (setf j-max (impulse-max motor dt)) 24 | ;; apply joint torque 25 | (decf (body-angular-velocity body-a) (* j-acc (body-inverse-inertia body-a))) 26 | (incf (body-angular-velocity body-b) (* j-acc (body-inverse-inertia body-b))))) 27 | 28 | (defmethod apply-impulse ((motor simple-motor)) 29 | (with-accessors ( 30 | (body-a simple-motor-body-a) 31 | (body-b simple-motor-body-b) 32 | (i-sum simple-motor-i-sum) 33 | (j-max simple-motor-j-max) 34 | (j-acc simple-motor-j-acc) 35 | (rate simple-motor-rate)) motor 36 | ;; compute relative rotational velocity 37 | (let* ((wr (+ (- (body-angular-velocity body-b) (body-angular-velocity body-a)) rate)) 38 | ;; compute normal impulse 39 | (j (* (- wr) i-sum)) 40 | (j-old j-acc)) 41 | (setf j-acc (clamp (+ j-old j) (- j-max) j-max)) 42 | (setf j (- j-acc j-old)) 43 | ;; apply impulse 44 | (decf (body-angular-velocity body-a) (* j (body-inverse-inertia body-a))) 45 | (incf (body-angular-velocity body-b) (* j (body-inverse-inertia body-b)))))) 46 | 47 | (defmethod get-impulse ((motor simple-motor)) 48 | (abs (simple-motor-j-acc motor))) 49 | -------------------------------------------------------------------------------- /src/constraints/damped-rotary-spring.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defstruct (damped-rotary-spring 5 | (:include spring) 6 | (:constructor make-damped-rotary-spring 7 | (body-a body-b rest-angle stiffness damping))) 8 | rest-angle damping target-wrn i-sum) 9 | 10 | (defmethod spring-torque ((spring damped-rotary-spring) relative-angle) 11 | (* (- relative-angle (damped-rotary-spring-rest-angle spring)) 12 | (damped-rotary-spring-stiffness spring))) 13 | 14 | (defmethod pre-step ((spring damped-rotary-spring) dt dt-inverse) 15 | (declare (ignore dt-inverse)) 16 | (let ((body-a (constraint-body-a spring)) 17 | (body-b (constraint-body-b spring))) 18 | (setf (damped-rotary-spring-i-sum spring) (/ (+ (body-inverse-inertia body-a) 19 | (body-inverse-inertia body-b))) 20 | (damped-rotary-spring-dt spring) dt 21 | (damped-rotary-spring-target-wrn spring) 0d0) 22 | 23 | ;; Applying spring torque 24 | (let ((j-spring (* dt (spring-torque spring (- (body-angle body-a) (body-angle body-b)))))) 25 | (decf (body-angular-velocity body-a) (* j-spring (body-inverse-inertia body-a))) 26 | (incf (body-angular-velocity body-b) (* j-spring (body-inverse-inertia body-b))))) 27 | (values)) 28 | 29 | (defmethod apply-impulse ((spring damped-rotary-spring)) 30 | (let* ((body-a (constraint-body-a spring)) 31 | (body-b (constraint-body-b spring)) 32 | (wrn (- (body-angular-velocity body-a) 33 | (body-angular-velocity body-b))) 34 | (angular-velocity-damp 35 | (* wrn (- 1d0 (exp (- (/ (* (damped-rotary-spring-damping spring) 36 | (damped-rotary-spring-dt spring)) 37 | (damped-rotary-spring-i-sum spring)))))))) 38 | (setf (damped-rotary-spring-target-wrn spring) (- wrn angular-velocity-damp)) 39 | (let ((j-damp (* angular-velocity-damp (damped-rotary-spring-i-sum spring)))) 40 | (decf (body-angular-velocity body-a) (* j-damp (body-inverse-inertia body-a))) 41 | (incf (body-angular-velocity body-b) (* j-damp (body-inverse-inertia body-b))))) 42 | (values)) 43 | -------------------------------------------------------------------------------- /demo/plink.lisp: -------------------------------------------------------------------------------- 1 | (in-package :squirl-demo) 2 | 3 | (defclass plink-demo (demo) 4 | ((num-verts :initarg :num-verts :initform 5 :accessor plink-num-verts) 5 | (static-body :initarg :static-body :initform (make-body :actor :not-grabbable) 6 | :accessor demo-static-body) 7 | (angle :accessor plink-angle)) 8 | (:default-initargs :name "Plink!" :physics-timestep 1/60)) 9 | 10 | (defmethod initialize-instance :after ((demo plink-demo) &key) 11 | (setf (plink-angle demo) (/ (* -2 pi) (plink-num-verts demo)))) 12 | 13 | (defun reset-fallen-body (body) 14 | (let* ((position (body-position body)) 15 | (x (vec-x position)) 16 | (y (vec-y position))) 17 | (when (or (< y -260) (> (abs x) 340)) 18 | (setf (body-position body) 19 | (vec (- (random 640) 320) 260))))) 20 | 21 | (defmethod update-demo :after ((demo plink-demo) dt) 22 | (declare (ignore dt)) 23 | (map-world #'reset-fallen-body (world demo))) 24 | 25 | (defun create-static-triangles (demo) 26 | (dotimes (i 9) 27 | (dotimes (j 6) 28 | (attach-shape (make-poly '#.(list (vec -15 -15) (vec 0 10) (vec 15 -15)) 29 | :offset (vec (- (* i 80) 320 (if (oddp j) -40 0)) 30 | (- (* j 70) 240)) 31 | :restitution 1 :friction 1) 32 | (demo-static-body demo))))) 33 | 34 | (defun create-polygons (demo) 35 | (let* ((verts (loop for i below (plink-num-verts demo) 36 | collect (vec* (angle->vec (* i (plink-angle demo))) 10d0))) 37 | (inertia (moment-of-inertia-for-poly 1 verts))) 38 | (dotimes (i 300) 39 | (world-add-body (world demo) 40 | (make-body :mass 1 :inertia inertia :position (vec (- (random 640) 320) 350) 41 | :shapes (list (make-poly verts :friction 0.2))))))) 42 | 43 | (defmethod init-demo ((demo plink-demo)) 44 | (setf (world demo) (make-world :iterations 5 :gravity (vec 0 -100))) 45 | (resize-world-active-hash (world demo) 40 999) 46 | (resize-world-static-hash (world demo) 30 2999) 47 | (world-add-body (world demo) (demo-static-body demo)) 48 | (create-static-triangles demo) 49 | (create-polygons demo) 50 | (world demo)) 51 | 52 | (pushnew 'plink-demo *demos*) 53 | -------------------------------------------------------------------------------- /demo/tumble.lisp: -------------------------------------------------------------------------------- 1 | (in-package :squirl-demo) 2 | 3 | (defclass tumble-demo (demo) 4 | ((box :initarg :box :accessor demo-box)) 5 | (:default-initargs :name "Tumbling along.")) 6 | 7 | (defmethod update-demo ((demo tumble-demo) dt) 8 | (incf (accumulator demo) (if (> dt *dt-threshold*) *dt-threshold* dt)) 9 | (loop while (>= (accumulator demo) (physics-timestep demo)) 10 | do (world-step (world demo) (physics-timestep demo)) 11 | ;; manually update the position of the box so that it rotates. 12 | (body-update-position (demo-box demo) (physics-timestep demo)) 13 | ;; Because the box was added as static and we moved it, we need to manually 14 | ;; rehash the static spatial hash 15 | (squirl::rehash-world-static-data (world demo)) 16 | (decf (accumulator demo) (physics-timestep demo)))) 17 | 18 | (defun build-demo-box () 19 | (let ((a (vec -200 -200)) 20 | (b (vec -200 200)) 21 | (c (vec 200 200)) 22 | (d (vec 200 -200))) 23 | (make-body :actor :not-grabbable 24 | :angular-velocity 0.4 25 | :shapes 26 | (list (make-segment a b :radius 0 :friction 1 :restitution 1) 27 | (make-segment b c :radius 0 :friction 1 :restitution 1) 28 | (make-segment c d :radius 0 :friction 1 :restitution 1) 29 | (make-segment d a :radius 0 :friction 1 :restitution 1))))) 30 | 31 | (defmethod init-demo ((demo tumble-demo)) 32 | (reset-shape-id-counter) 33 | (setf (world demo) (make-world :gravity (vec 0 -600))) 34 | (resize-world-active-hash (world demo) 30 999) 35 | (resize-world-static-hash (world demo) 200 99) 36 | (setf (demo-box demo) (world-add-body (world demo) (build-demo-box))) 37 | ;; add the bricks 38 | (let* ((verts (list (vec -30 -15) 39 | (vec -30 15) 40 | (vec 30 15) 41 | (vec 30 -15))) 42 | (inertia (moment-of-inertia-for-poly 1 verts))) 43 | (dotimes (i 3) 44 | (dotimes (j 7) 45 | (world-add-body (world demo) 46 | (make-body :mass 1 :inertia inertia :position (vec (- (* i 60) 150) 47 | (- (* j 30) 150)) 48 | :shapes (list (make-poly verts :friction 0.7))))))) 49 | (world demo)) 50 | 51 | (pushnew 'tumble-demo *demos*) 52 | -------------------------------------------------------------------------------- /demo/planet.lisp: -------------------------------------------------------------------------------- 1 | (in-package :squirl-demo) 2 | 3 | (defclass planet-demo (demo) 4 | ((planet :initarg :planet :accessor planet)) 5 | (:default-initargs :name "Planetary Gravity OMFG.")) 6 | 7 | (defmethod update-demo ((demo planet-demo) dt) 8 | (incf (accumulator demo) (if (> dt *dt-threshold*) *dt-threshold* dt)) 9 | (loop while (>= (accumulator demo) (physics-timestep demo)) 10 | do (world-step (world demo) (physics-timestep demo)) 11 | (body-update-position (planet demo) (physics-timestep demo)) 12 | (decf (accumulator demo) (physics-timestep demo)))) 13 | 14 | (defbody planetary-body) 15 | 16 | (defmethod body-update-velocity ((body planetary-body) gravity damping dt) 17 | (declare (ignore gravity)) 18 | (let* ((position (body-position body)) 19 | (gravity (vec* position (/ -50000 (vec. position position))))) 20 | (call-next-method body gravity damping dt))) 21 | 22 | (defun random-position (radius) 23 | (loop for vec = (vec (- (random (- 640 (* 2 radius))) 24 | (- 320 radius)) 25 | (- (random (- 480 (* 2 radius))) 26 | (- 240 radius))) 27 | when (< 88 (vec-length vec) 200) 28 | return vec)) 29 | 30 | (let ((size 10) (mass 1)) 31 | (defun add-box () 32 | (let* ((verts (list (vec (- size) (- size)) 33 | (vec (- size) size) 34 | (vec size size) 35 | (vec size (- size))))) 36 | (world-add-body 37 | (world *current-demo*) 38 | (make-planetary-body :mass mass 39 | :position (random-position (vec-length (vec size size))) 40 | :velocity (vec* (angle->vec (* pi (random 2d0))) 41 | (random 200d0)) 42 | :shapes (list 43 | (make-poly verts :friction 0.7 :restitution 1))))))) 44 | 45 | (defmethod init-demo ((demo planet-demo)) 46 | (setf (planet demo) 47 | (make-body :angular-velocity 0.3 :actor :not-grabbable 48 | :shapes (list (make-circle 70 :restitution 1 :friction 0.8)))) 49 | (reset-shape-id-counter) 50 | (setf (world demo) (make-world :iterations 20)) 51 | (loop repeat 22 do (add-box)) 52 | (world-add-body (world demo) (planet demo)) 53 | (world demo)) 54 | 55 | (pushnew 'planet-demo *demos*) 56 | -------------------------------------------------------------------------------- /src/constraints/gear-joint.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defstruct (gear-joint (:include constraint) 5 | (:constructor 6 | make-gear-joint 7 | (body-a body-b phase ratio 8 | &aux (ratio-inverse (/ ratio))))) 9 | phase 10 | ratio 11 | ratio-inverse 12 | (i-sum 0d0) 13 | (bias 0d0) 14 | (j-acc 0d0) 15 | (j-max 0d0)) 16 | 17 | (defmethod pre-step ((gear gear-joint) dt dt-inv) 18 | (with-accessors ( 19 | (body-a gear-joint-body-a) 20 | (body-b gear-joint-body-b) 21 | (i-sum gear-joint-i-sum) 22 | (j-max gear-joint-j-max) 23 | (bias gear-joint-bias) 24 | (ratio gear-joint-ratio) 25 | (max-bias gear-joint-max-bias) 26 | (bias-coef gear-joint-bias-coefficient) 27 | (ratio-inverse gear-joint-ratio-inverse) 28 | (j-acc gear-joint-j-acc) 29 | (phase gear-joint-phase)) gear 30 | ;; calculate moment of inertia coefficient 31 | (setf i-sum (/ 1d0 (+ (* (body-inverse-inertia body-a) ratio-inverse) (* ratio (body-inverse-inertia body-b))))) 32 | ;; calculate bias velocity 33 | (setf bias (clamp (- (* bias-coef dt-inv (- (* (body-angle body-b) ratio) (body-angle body-a) phase))) (- max-bias) max-bias)) 34 | ;; compute max impulse 35 | (setf j-max (impulse-max gear dt)) 36 | ;; apply joint torque 37 | (decf (body-angular-velocity body-a) (* j-acc (body-inverse-inertia body-a) ratio-inverse)) 38 | (incf (body-angular-velocity body-b) (* j-acc (body-inverse-inertia body-b))))) 39 | 40 | (defmethod apply-impulse ((gear gear-joint)) 41 | (with-accessors ( 42 | (body-a gear-joint-body-a) 43 | (body-b gear-joint-body-b) 44 | (ratio-inverse gear-joint-ratio-inverse) 45 | (ratio gear-joint-ratio) 46 | (j-max gear-joint-j-max) 47 | (i-sum gear-joint-i-sum) 48 | (j-acc gear-joint-j-acc) 49 | (bias gear-joint-bias)) gear 50 | ;; compute relative rotational velocity 51 | (let* ((wr (- (* (body-angular-velocity body-b) ratio) (body-angular-velocity body-a))) 52 | ;; compute normal impulse 53 | (j (* (- bias wr) i-sum)) 54 | (j-old j-acc)) 55 | (setf j-acc (clamp (+ j-old j) (- j-max) j-max)) 56 | (setf j (- j-acc j-old)) 57 | ;; apply impulse 58 | (decf (body-angular-velocity body-a) (* j (body-inverse-inertia body-a) ratio-inverse)) 59 | (incf (body-angular-velocity body-b) (* j (body-inverse-inertia body-b)))))) 60 | 61 | (defmethod get-impulse ((gear gear-joint)) 62 | (abs (gear-joint-j-acc gear))) 63 | -------------------------------------------------------------------------------- /src/constraints/pivot-joint.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defstruct (pivot-joint (:include constraint) 5 | (:constructor 6 | make-pivot-joint 7 | (body-a body-b anchor1 anchor2))) 8 | (anchor1 +zero-vector+ :type vec) 9 | (anchor2 +zero-vector+ :type vec) 10 | 11 | (r1 +zero-vector+ :type vec) 12 | (r2 +zero-vector+ :type vec) 13 | (k1 +zero-vector+ :type vec) 14 | (k2 +zero-vector+ :type vec) 15 | 16 | (j-max-length 0d0) 17 | (bias +zero-vector+ :type vec) 18 | (j-acc +zero-vector+ :type vec)) 19 | 20 | (defmethod pre-step ((pivot pivot-joint) dt dt-inv) 21 | (with-accessors ((body-a pivot-joint-body-a) 22 | (body-b pivot-joint-body-b) 23 | (j-max-length pivot-joint-j-max-length) 24 | (bias pivot-joint-bias) 25 | (bias-coef pivot-joint-bias-coefficient) 26 | (j-acc pivot-joint-j-acc) 27 | (r1 pivot-joint-r1) 28 | (r2 pivot-joint-r2) 29 | (k1 pivot-joint-k1) 30 | (k2 pivot-joint-k2) 31 | (max-bias pivot-joint-max-bias) 32 | (anchor1 pivot-joint-anchor1) 33 | (anchor2 pivot-joint-anchor2)) 34 | pivot 35 | (setf r1 (vec-rotate anchor1 (body-rotation body-a))) 36 | (setf r2 (vec-rotate anchor2 (body-rotation body-b))) 37 | ;; calculate mass tensor 38 | (multiple-value-bind (new-k1 new-k2) (k-tensor body-a body-b r1 r2) 39 | (setf k1 new-k1 k2 new-k2)) 40 | ;; compute max impulse 41 | (setf j-max-length (impulse-max pivot dt)) 42 | ;; calculate bias velocity 43 | (let ((delta (vec- (vec+ (body-position body-b) r2) 44 | (vec+ (body-position body-a) r1)))) 45 | (setf bias (vec-clamp (vec* delta (- (* bias-coef dt-inv))) 46 | max-bias))) 47 | ;; apply joint torque 48 | (apply-impulses body-a body-b r1 r2 j-acc))) 49 | 50 | (defmethod apply-impulse ((pivot pivot-joint)) 51 | (with-accessors ((body-a pivot-joint-body-a) 52 | (body-b pivot-joint-body-b) 53 | (j-max-length pivot-joint-j-max-length) 54 | (bias pivot-joint-bias) 55 | (bias-coef pivot-joint-bias-coefficient) 56 | (j-acc pivot-joint-j-acc) 57 | (r1 pivot-joint-r1) 58 | (r2 pivot-joint-r2) 59 | (k1 pivot-joint-k1) 60 | (k2 pivot-joint-k2)) pivot 61 | ;; compute relative velocity 62 | (let* ((vr (relative-velocity body-a body-b r1 r2)) 63 | ;; compute normal impulse 64 | (j (mult-k (vec- bias vr) k1 k2)) 65 | (j-old j-acc)) 66 | (setf j-acc (vec-clamp (vec+ j-acc j) j-max-length)) 67 | (setf j (vec- j-acc j-old)) 68 | ;; apply impulse 69 | (apply-impulses body-a body-b r1 r2 j)))) 70 | 71 | (defmethod get-impulse ((pivot pivot-joint)) 72 | (vec-length (pivot-joint-j-acc pivot))) 73 | -------------------------------------------------------------------------------- /src/constraints/ratchet-joint.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defstruct (ratchet-joint (:include constraint) 5 | (:constructor 6 | make-ratchet-joint 7 | (body-a body-b direction))) 8 | direction 9 | angle 10 | (i-sum 0d0) 11 | (bias 0d0) 12 | (j-acc 0d0) 13 | (j-max 0d0)) 14 | 15 | (defmethod pre-step ((ratchet ratchet-joint) dt dt-inv) 16 | (with-accessors ( 17 | (body-a ratchet-joint-body-a) 18 | (body-b ratchet-joint-body-b) 19 | (i-sum ratchet-joint-i-sum) 20 | (j-max ratchet-joint-j-max) 21 | (bias ratchet-joint-bias) 22 | (bias-coef ratchet-joint-bias-coefficient) 23 | (direction ratchet-joint-direction) 24 | (j-acc ratchet-joint-j-acc) 25 | (angle ratchet-joint-angle) 26 | (max-bias ratchet-joint-max-bias)) ratchet 27 | (let* ((delta (- (body-angle body-b) (body-angle body-a))) 28 | (diff (- angle delta)) 29 | (pdist (if (> diff 0d0) diff 0d0))) 30 | (setf angle (* direction (max (* delta direction) (* angle direction)))) 31 | ;; calculate moment of inertia coefficient 32 | (setf i-sum (/ 1d0 (+ (body-inverse-inertia body-a) (body-inverse-inertia body-b)))) 33 | ;; calculate bias velocity 34 | (setf bias (clamp (- (* bias-coef dt-inv pdist)) (- max-bias) max-bias)) 35 | ;; compute max impulse 36 | (setf j-max (impulse-max ratchet dt)) 37 | ;; if the bias is zero, the joint is not at a limit, reset impulse 38 | (when (zerop bias) 39 | (setf j-acc 0d0)) 40 | ;; apply joint torque 41 | (decf (body-angular-velocity body-a) (* j-acc (body-inverse-inertia body-a))) 42 | (incf (body-angular-velocity body-b) (* j-acc (body-inverse-inertia body-b)))))) 43 | 44 | (defmethod apply-impulse ((ratchet ratchet-joint)) 45 | (with-accessors ( 46 | (body-a ratchet-joint-body-a) 47 | (body-b ratchet-joint-body-b) 48 | (direction ratchet-joint-direction) 49 | (j-max ratchet-joint-j-max) 50 | (i-sum ratchet-joint-i-sum) 51 | (j-acc ratchet-joint-j-acc) 52 | (bias ratchet-joint-bias)) ratchet 53 | (when (zerop bias) (return-from apply-impulse)) 54 | ;; compute relative rotational velocity 55 | (let* ((wr (- (body-angular-velocity body-b) (body-angular-velocity body-a))) 56 | ;; compute normal impulse 57 | (j (* (- (+ bias wr)) i-sum)) 58 | (j-old j-acc)) 59 | (setf j-acc (* (clamp (* (+ j-old j) direction) 0d0 j-max) direction)) 60 | (setf j (- j-acc j-old)) 61 | ;; apply impulse 62 | (decf (body-angular-velocity body-a) (* j (body-inverse-inertia body-a))) 63 | (incf (body-angular-velocity body-b) (* j (body-inverse-inertia body-b)))))) 64 | 65 | (defmethod get-impulse ((joint ratchet-joint)) 66 | (abs (ratchet-joint-j-acc joint))) 67 | -------------------------------------------------------------------------------- /src/constraints/damped-spring.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defstruct (damped-spring (:include spring) 5 | (:constructor 6 | make-damped-spring 7 | (body-a body-b anchor1 anchor2 rest-length stiffness damping))) 8 | anchor1 anchor2 rest-length damping target-vrn r1 r2 n-mass normal) 9 | 10 | (defmethod spring-force ((spring damped-spring) distance) 11 | (* (- (damped-spring-rest-length spring) distance) 12 | (spring-stiffness spring))) 13 | 14 | (defmethod pre-step ((spring damped-spring) dt dt-inverse) 15 | (declare (ignore dt-inverse)) 16 | (let ((body-a (spring-body-a spring)) 17 | (body-b (spring-body-b spring))) 18 | (setf (damped-spring-r1 spring) (vec-rotate (damped-spring-anchor1 spring) 19 | (body-rotation body-a)) 20 | (damped-spring-r2 spring) (vec-rotate (damped-spring-anchor2 spring) 21 | (body-rotation body-b))) 22 | (let* ((delta (vec- (vec+ (body-position body-b) (damped-spring-r2 spring)) 23 | (vec+ (body-position body-a) (damped-spring-r1 spring)))) 24 | (distance (vec-length delta))) 25 | (setf (damped-spring-normal spring) (vec* delta (maybe/ 1d0 distance)) 26 | ;; calculate mass normal 27 | (damped-spring-n-mass spring) (/ (k-scalar body-a body-b 28 | (damped-spring-r1 spring) 29 | (damped-spring-r2 spring) 30 | (damped-spring-normal spring))) 31 | (spring-dt spring) dt 32 | (damped-spring-target-vrn spring) 0d0) 33 | ;; apply spring force. 34 | (apply-impulses body-a body-b (damped-spring-r1 spring) (damped-spring-r2 spring) 35 | (vec* (damped-spring-normal spring) 36 | (* dt (spring-force spring distance)))) 37 | (values)))) 38 | 39 | (defmethod apply-impulse ((spring damped-spring)) 40 | (let* ((body-a (spring-body-a spring)) 41 | (body-b (spring-body-b spring)) 42 | (normal (damped-spring-normal spring)) 43 | (r1 (damped-spring-r1 spring)) 44 | (r2 (damped-spring-r2 spring)) 45 | (n-mass (damped-spring-n-mass spring)) 46 | ;; compute relative velocity 47 | (vrn (- (normal-relative-velocity body-a body-b r1 r2 normal) 48 | (damped-spring-target-vrn spring))) 49 | ;; compute velocity loss from drag. 50 | ;; C source sez: "not 100% certain this is derived correctly, though it makes sense" 51 | (v-damp (- (* vrn (- 1d0 (exp (- (/ (* (damped-spring-damping spring) 52 | (damped-spring-dt spring)) 53 | n-mass)))))))) 54 | (setf (damped-spring-target-vrn spring) (+ vrn v-damp)) 55 | (apply-impulses body-a body-b r1 r2 (vec* normal (* v-damp n-mass))) 56 | (values))) 57 | -------------------------------------------------------------------------------- /src/constraints/rotary-limit-joint.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defstruct (rotary-limit-joint (:include constraint) 5 | (:constructor 6 | make-rotary-limit-joint 7 | (body-a body-b min max))) 8 | min 9 | max 10 | (i-sum 0d0) 11 | (bias 0d0) 12 | (j-acc 0d0) 13 | (j-max 0d0)) 14 | 15 | (defmethod pre-step ((rotary-limit rotary-limit-joint) dt dt-inv) 16 | (with-accessors ( 17 | (body-a rotary-limit-joint-body-a) 18 | (body-b rotary-limit-joint-body-b) 19 | (i-sum rotary-limit-joint-i-sum) 20 | (j-max rotary-limit-joint-j-max) 21 | (bias rotary-limit-joint-bias) 22 | (bias-coef rotary-limit-joint-bias-coefficient) 23 | (min rotary-limit-joint-min) 24 | (j-acc rotary-limit-joint-j-acc) 25 | (max rotary-limit-joint-max) 26 | (max-bias rotary-limit-joint-max-bias)) rotary-limit 27 | (let* ((dist (- (body-angle body-b) (body-angle body-a))) 28 | (pdist (if (> dist max) (- max dist) (- min dist)))) 29 | ;; calculate moment of inertia coefficient 30 | (setf i-sum (/ 1d0 (+ (body-inverse-inertia body-a) (body-inverse-inertia body-b)))) 31 | ;; calculate bias velocity 32 | (setf bias (clamp (- (* bias-coef dt-inv pdist)) (- max-bias) max-bias)) 33 | ;; compute max impulse 34 | (setf j-max (impulse-max rotary-limit dt)) 35 | ;; if the bias is zero, the joint is not at a limit, reset impulse 36 | (when (zerop bias) 37 | (setf j-acc 0d0)) 38 | ;; apply joint torque 39 | (decf (body-angular-velocity body-a) (* j-acc (body-inverse-inertia body-a))) 40 | (incf (body-angular-velocity body-b) (* j-acc (body-inverse-inertia body-b)))))) 41 | 42 | (defmethod apply-impulse ((rotary-limit rotary-limit-joint)) 43 | (with-accessors ( 44 | (body-a rotary-limit-joint-body-a) 45 | (body-b rotary-limit-joint-body-b) 46 | (direction rotary-limit-joint-direction) 47 | (j-max rotary-limit-joint-j-max) 48 | (i-sum rotary-limit-joint-i-sum) 49 | (j-acc rotary-limit-joint-j-acc) 50 | (bias rotary-limit-joint-bias)) rotary-limit 51 | (when (zerop bias) (return-from apply-impulse)) 52 | ;; compute relative rotational velocity 53 | (let* ((wr (- (body-angular-velocity body-b) (body-angular-velocity body-a))) 54 | ;; compute normal impulse 55 | (j (* (- (+ bias wr)) i-sum)) 56 | (j-old j-acc)) 57 | (if (< bias 0d0) 58 | (setf j-acc (clamp (+ j-old j) 0d0 j-max)) 59 | (setf j-acc (clamp (+ j-old j) (- j-max) 0d0))) 60 | (setf j (- j-acc j-old)) 61 | ;; apply impulse 62 | (decf (body-angular-velocity body-a) (* j (body-inverse-inertia body-a))) 63 | (incf (body-angular-velocity body-b) (* j (body-inverse-inertia body-b)))))) 64 | 65 | (defmethod get-impulse ((rotary rotary-limit-joint)) 66 | (abs (rotary-limit-joint-j-acc rotary))) 67 | -------------------------------------------------------------------------------- /demo/pyramid.lisp: -------------------------------------------------------------------------------- 1 | (in-package :squirl-demo) 2 | 3 | (defparameter *step* (/ 1 60 2)) 4 | 5 | (defclass pyramid-demo (demo) 6 | ((floor :accessor demo-floor)) 7 | (:default-initargs :name "Pyramid Topple")) 8 | 9 | ;; uncomment this bit to let the pyramid demo run at maximum speed. 10 | #+nil(defmethod update-demo ((demo pyramid-demo) dt) 11 | (declare (ignore dt)) 12 | (world-step (world demo) (physics-timestep demo))) 13 | 14 | (defmethod init-demo ((demo pyramid-demo)) 15 | (reset-shape-id-counter) 16 | (setf (world demo) (make-world :iterations 20 17 | :gravity (vec 0 -300))) 18 | (resize-world-active-hash (world demo) 40.0 2999) 19 | (resize-world-static-hash (world demo) 40.0 999) 20 | (setf (demo-floor demo) (make-body :actor :not-grabbable 21 | :shapes (list (make-segment (vec -600 -240) 22 | (vec 600 -240) 23 | :restitution 1 24 | :friction 1)))) 25 | (world-add-body (world demo) (demo-floor demo)) 26 | (let ((friction 0.6) 27 | (verts (list (vec -3 -20) 28 | (vec -3 20) 29 | (vec 3 20) 30 | (vec 3 -20)))) 31 | (loop 32 | with n = 9 33 | for i from 1 to n 34 | for offset = (vec (- (/ (* i 60) 2)) 35 | (* (- n i) 52)) 36 | do (loop 37 | for j from 0 below i do 38 | (mapc (lambda (body) 39 | (attach-shape (make-poly verts :friction friction) 40 | body) 41 | (world-add-body (world demo) body)) 42 | (nconc 43 | (list (make-body 44 | :mass 1 45 | :position (vec+ (vec (* j 60) -220) 46 | offset)) 47 | (make-body 48 | :mass 1 49 | :position (vec+ (vec (* j 60) -197) 50 | offset) 51 | :angle (/ pi 2))) 52 | (unless (= j (1- i)) 53 | (list (make-body 54 | :mass 1 55 | :position (vec+ (vec (+ (* j 60) 30) 56 | -191) 57 | offset) 58 | :angle (/ pi 2))))))) 59 | (mapc (lambda (body) 60 | (attach-shape (make-poly verts :friction friction) 61 | body) 62 | (world-add-body (world demo) body)) 63 | (list (make-body 64 | :mass 1 65 | :position (vec+ (vec -17 66 | -174) 67 | offset)) 68 | (make-body 69 | :mass 1 70 | :position (vec+ (vec (+ (* (1- i) 60) 17) 71 | -174) 72 | offset)))))) 73 | (world demo)) 74 | 75 | (pushnew 'pyramid-demo *demos*) 76 | -------------------------------------------------------------------------------- /src/constraints/slide-joint.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defstruct (slide-joint (:include constraint) 5 | (:constructor 6 | make-slide-joint 7 | (body-a body-b anchor1 anchor2))) 8 | (anchor1 +zero-vector+ :type vec) 9 | (anchor2 +zero-vector+ :type vec) 10 | (min-slide 0d0) 11 | (max-slide 0d0) 12 | (r1 +zero-vector+ :type vec) 13 | (r2 +zero-vector+ :type vec) 14 | (n +zero-vector+ :type vec) 15 | (n-mass 0d0) 16 | (jn-acc 0d0) 17 | (jn-max 0d0) 18 | (bias 0d0)) 19 | 20 | (defmethod pre-step ((joint slide-joint) dt dt-inv) 21 | (with-accessors ( 22 | (bias-coef slide-joint-bias-coefficient) 23 | (max-bias slide-joint-max-bias) 24 | (body-a slide-joint-body-a) 25 | (body-b slide-joint-body-b) 26 | (anchor1 slide-joint-anchor1) 27 | (anchor2 slide-joint-anchor2) 28 | (min-slide slide-joint-min-slide) 29 | (max-slide slide-joint-max-slide) 30 | (r1 slide-joint-r1) 31 | (r2 slide-joint-r2) 32 | (n slide-joint-n) 33 | (n-mass slide-joint-n-mass) 34 | (bias slide-joint-bias) 35 | (jn-max slide-joint-jn-max) 36 | (jn-acc slide-joint-jn-acc)) joint 37 | 38 | (let* ((delta (vec- (vec+ (body-position body-b) r2) (vec+ (body-position body-a) r1))) 39 | (dist (vec-length delta)) 40 | (pdist 0d0)) 41 | (setf r1 (vec-rotate anchor1 body-a)) 42 | (setf r2 (vec-rotate anchor2 body-b)) 43 | (if (> dist max-slide) 44 | (setf pdist (- dist max-slide)) 45 | (progn 46 | (setf pdist (- min-slide dist)) 47 | (setf dist (- dist)))) 48 | (setf n (vec* delta (maybe/ 1d0 dist))) 49 | ;; calculate mass normal 50 | (setf n-mass (/ 1d0 (k-scalar body-a body-b r1 r2 n ))) 51 | ;; calculate bias velocity 52 | (setf bias (clamp (- (* bias-coef dt-inv pdist)) (- max-bias) max-bias)) 53 | ;; compute max impulse 54 | (setf jn-max (impulse-max joint dt)) 55 | ;;apply accumulated impulse 56 | (when (zerop bias) 57 | ;; if bias is 0, then the joint is not at a limit 58 | (setf jn-acc 0d0)) 59 | (apply-impulses body-a body-b r1 r2 (vec* n jn-acc))))) 60 | 61 | (defmethod apply-impulse (joint) 62 | (with-accessors ( 63 | (body-a slide-joint-body-a) 64 | (body-b slide-joint-body-b) 65 | (r1 slide-joint-r1) 66 | (r2 slide-joint-r2) 67 | (n slide-joint-n) 68 | (n-mass slide-joint-n-mass) 69 | (bias slide-joint-bias) 70 | (jn-max slide-joint-jn-max) 71 | (jn-acc slide-joint-jn-acc)) joint 72 | (when (zerop bias) 73 | (return-from apply-impulse)) 74 | ;; compute relative velocity 75 | (let* ((vr (relative-velocity body-a body-b r1 r2)) 76 | (vrn (vec. vr n)) 77 | ;; compute normal impulse 78 | (jn (* (- bias vrn) n-mass)) 79 | (jn-old jn-acc)) 80 | (setf jn-acc (clamp (+ jn-old jn) (- jn-max) 0d0)) 81 | (setf jn (- jn-acc jn-old)) 82 | ;; apply impulse 83 | (apply-impulses body-a body-b r1 r2 (vec* n jn))))) 84 | 85 | (defmethod get-impulse ((joint slide-joint)) 86 | (abs (slide-joint-jn-acc joint))) -------------------------------------------------------------------------------- /squirl.asd: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; Syntax: ANSI-Common-Lisp; Base: 10; indent-tabs-mode: nil -*- 2 | 3 | (asdf:defsystem squirl 4 | :version "0.1 (unreleased)" 5 | :maintainer "Kat Marchán " 6 | :licence "MIT" 7 | :components 8 | ((:module "src" 9 | :components 10 | ((:file "arbiter" :depends-on ("vec" "shape" "collision" "contact")) 11 | (:file "body" :depends-on ("vec")) 12 | (:file "bounding-box" :depends-on ("vec")) 13 | (:file "collision" :depends-on ("shape" "poly-shape" "contact")) 14 | (:file "contact" :depends-on ("vec")) 15 | (:file "convenience" :depends-on ("body" "shape" "poly-shape" "vec")) 16 | (:file "hash-set" :depends-on ("utils")) 17 | (:file "package") 18 | (:file "shape" :depends-on ("vec" "bounding-box" "body")) 19 | (:file "poly-shape" :depends-on ("shape")) 20 | (:file "squirl" :depends-on ("vec")) 21 | (:file "utils" :depends-on ("package")) 22 | (:file "vec" :depends-on ("utils")) 23 | (:file "world" :depends-on ("vec" "arbiter" "body")) 24 | (:file "world-hash" :depends-on ("hash-set" "vec")) 25 | (:module "constraints" :depends-on ("shape" "poly-shape" "arbiter") 26 | :components 27 | ((:file "breakable-joint" :depends-on ("constraints")) 28 | (:file "constraints" :depends-on ("util")) 29 | (:file "damped-rotary-spring" :depends-on ("spring")) 30 | (:file "damped-spring" :depends-on ("spring")) 31 | (:file "gear-joint" :depends-on ("constraints")) 32 | (:file "groove-joint" :depends-on ("constraints")) 33 | (:file "pin-joint" :depends-on ("constraints")) 34 | (:file "pivot-joint" :depends-on ("constraints")) 35 | (:file "ratchet-joint" :depends-on ("constraints")) 36 | (:file "rotary-limit-joint" :depends-on ("constraints")) 37 | (:file "simple-motor" :depends-on ("constraints")) 38 | (:file "slide-joint" :depends-on ("constraints")) 39 | (:file "spring" :depends-on ("constraints")) 40 | (:file "util"))))))) 41 | 42 | (asdf:defsystem squirl.demo 43 | :version "0.1 (unreleased)" 44 | :maintainer "Kat Marchán " 45 | :licence "MIT" 46 | :depends-on (:squirl :cl-opengl :cl-glu :cl-glut) 47 | :components 48 | ((:module "demo" 49 | :components 50 | ((:file "squirl-demo") 51 | (:file "draw-world" :depends-on ("squirl-demo")) 52 | (:file "logo-smash" :depends-on ("squirl-demo")) 53 | (:file "planet" :depends-on ("squirl-demo")) 54 | (:file "tumble" :depends-on ("squirl-demo")) 55 | (:file "plink" :depends-on ("squirl-demo")) 56 | (:file "pump" :depends-on ("squirl-demo")) 57 | (:file "pyramid" :depends-on ("squirl-demo")) 58 | (:file "pyramid-stack" :depends-on ("squirl-demo")) 59 | (:file "theo-jansen" :depends-on ("squirl-demo")) 60 | (:file "springies" :depends-on ("squirl-demo")))))) 61 | 62 | (asdf:defsystem squirl.demo-2 63 | :version "0.1 (unreleased)" 64 | :maintainer "Michael Compton " 65 | :licence "MIT" 66 | :depends-on (:squirl :lispbuilder-sdl) 67 | :components 68 | ((:module "demo" 69 | :components 70 | ((:file "demo"))))) 71 | -------------------------------------------------------------------------------- /src/constraints/pin-joint.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defstruct (pin-joint (:include constraint) 5 | (:constructor 6 | make-pin-joint 7 | (body-a body-b anchor1 anchor2 &aux 8 | (point-1 (vec+ (body-position body-b) 9 | (vec-rotate anchor1 (body-rotation body-b)))) 10 | (point-2 (vec+ (body-position body-a) 11 | (vec-rotate anchor2 (body-rotation body-a)))) 12 | (distance (vec-length (vec- point-1 point-2)))))) 13 | anchor1 anchor2 14 | distance 15 | r1 r2 16 | normal 17 | n-mass 18 | (jn-acc 0d0) 19 | jn-max 20 | bias) 21 | 22 | (defmethod pre-step ((joint pin-joint) dt dt-inverse) 23 | (let ((body-a (pin-joint-body-a joint)) 24 | (body-b (pin-joint-body-b joint))) 25 | (setf (pin-joint-r1 joint) (vec-rotate (pin-joint-anchor1 joint) 26 | (body-rotation body-a)) 27 | (pin-joint-r2 joint) (vec-rotate (pin-joint-anchor2 joint) 28 | (body-rotation body-b))) 29 | (let* ((delta (vec- (vec+ (body-position body-b) 30 | (pin-joint-r2 joint)) 31 | (vec+ (body-position body-a) 32 | (pin-joint-r2 joint)))) 33 | (distance (vec-length delta)) 34 | (max-bias (pin-joint-max-bias joint))) 35 | (setf (pin-joint-normal joint) (vec* delta (/ (if (zerop distance) 36 | most-positive-double-float 37 | distance))) 38 | (pin-joint-n-mass joint) (/ (k-scalar body-a body-b 39 | (pin-joint-r1 joint) 40 | (pin-joint-r2 joint) 41 | (pin-joint-normal joint))) 42 | (pin-joint-bias joint) (clamp (- (* (pin-joint-bias-coefficient joint) 43 | dt-inverse 44 | (- distance (pin-joint-distance joint)))) 45 | (- max-bias) max-bias) 46 | (pin-joint-jn-max joint) (impulse-max joint dt)) 47 | (apply-impulses body-a body-b (pin-joint-r1 joint) (pin-joint-r2 joint) 48 | (vec* (pin-joint-normal joint) (pin-joint-jn-acc joint))))) 49 | (values)) 50 | 51 | (defmethod apply-impulse ((joint pin-joint)) 52 | (let* ((body-a (pin-joint-body-a joint)) 53 | (body-b (pin-joint-body-b joint)) 54 | (normal (pin-joint-normal joint)) 55 | ;; compute relative velocity 56 | (relative-velocity (normal-relative-velocity body-a body-b 57 | (pin-joint-r1 joint) 58 | (pin-joint-r2 joint) 59 | normal)) 60 | ;; copmute normal impulse 61 | (jn (* (- (pin-joint-bias joint) relative-velocity) 62 | (pin-joint-n-mass joint))) 63 | (jn-old (pin-joint-jn-acc joint))) 64 | (setf (pin-joint-jn-acc joint) (clamp (+ jn jn-old) 65 | (- (pin-joint-jn-max joint)) 66 | (pin-joint-jn-max joint)) 67 | jn (- (pin-joint-jn-acc joint) jn-old)) 68 | (apply-impulses body-a body-b (pin-joint-r1 joint) (pin-joint-r2 joint) (vec* normal jn))) 69 | (values)) 70 | 71 | (defmethod get-impulse ((joint pin-joint)) 72 | (abs (pin-joint-jn-acc joint))) 73 | -------------------------------------------------------------------------------- /src/package.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | 3 | (in-package :cl-user) 4 | 5 | (defpackage squirl.utils 6 | (:use :cl) 7 | (:export :make-adjustable-vector 8 | :ensure-list 9 | :clamp 10 | :maybe/ 11 | :maybe-inverse 12 | :fun :_ 13 | :deletef 14 | :delete-iff 15 | :with-gensyms 16 | :without-floating-point-underflow 17 | :symbolicate 18 | :ensure-car 19 | :ensure-cadr 20 | :push-cons 21 | :define-constant 22 | :define-print-object 23 | :do-vector 24 | :with-place 25 | :aprog1 :aif :awhen :it 26 | :parse-defmethod 27 | :pop-declarations)) 28 | 29 | (defpackage squirl 30 | (:use :cl :squirl.utils) 31 | (:export 32 | 33 | ;; Vector math 34 | :vec 35 | :vec-x 36 | :vec-y 37 | :vec+ 38 | :vec- 39 | :vec* 40 | :+zero-vector+ 41 | :vec-length 42 | :vec-lerp 43 | :vec-zerop 44 | :vec-equal 45 | :angle->vec 46 | :vec->angle 47 | :vec. 48 | :vec-cross 49 | :vec-perp 50 | :vec-rperp 51 | :vec-project 52 | :vec-rotate 53 | :vec-unrotate 54 | :vec-length-sq 55 | :vec-normalize 56 | :vec-clamp 57 | :vec-dist 58 | :vec-dist-sq 59 | :vec-near 60 | 61 | ;; Bodies 62 | :body 63 | :defbody 64 | :make-body 65 | :body-world 66 | :body-actor 67 | :body-rotation 68 | :body-angle 69 | :body-position 70 | :body-velocity 71 | :body-force 72 | :body-shapes 73 | :staticp 74 | :body-update-velocity 75 | :body-update-position 76 | :body-slew 77 | :body-local->world 78 | :world->body-local 79 | :body-reset-forces 80 | :body-apply-force 81 | :apply-damped-spring 82 | 83 | ;; Shapes 84 | :reset-shape-id-counter 85 | :shape-body 86 | :shape-restitution 87 | :shape-friction 88 | :shape-layers 89 | :circle 90 | :make-circle 91 | :circle-radius 92 | :circle-transformed-center 93 | :segment 94 | :make-segment 95 | :segment-trans-a 96 | :segment-trans-b 97 | :poly 98 | :make-poly 99 | :poly-transformed-vertices 100 | :attach-shape 101 | :attach-shapes 102 | :detach-shape 103 | 104 | ;; moments 105 | :moment-of-inertia-for-circle 106 | :moment-of-inertia-for-segment 107 | :moment-of-inertia-for-poly 108 | 109 | ;; world 110 | :world 111 | :make-world 112 | :map-world 113 | :map-world-hash 114 | :world-bodies 115 | :world-constraints 116 | :world-add-body 117 | :world-add-shape 118 | :world-add-static-shape 119 | :world-add-constraint 120 | :world-remove-shape 121 | :world-remove-static-shape 122 | :world-remove-body 123 | :world-remove-constraint 124 | :world-active-shapes 125 | :world-static-shapes 126 | :resize-world-static-hash 127 | :resize-world-active-hash 128 | :rehash-world-static-data 129 | :world-point-query-first 130 | :world-step 131 | 132 | ;; constraints 133 | :body-a 134 | :body-b 135 | :anchor1 136 | :anchor2 137 | :rest-length 138 | :stiffness 139 | :damping 140 | :constraint-body-a 141 | :constraint-body-b 142 | :breakable-joint 143 | :make-breakable-joint 144 | :pivot-joint 145 | :make-pivot-joint 146 | :damped-rotary-spring 147 | :make-damped-rotary-spring 148 | :damped-spring 149 | :make-damped-spring 150 | :spring-stiffness 151 | :gear-joint 152 | :make-gear-joint 153 | :groove-joint 154 | :make-groove-joint 155 | :pin-joint 156 | :make-pin-joint 157 | :pivot-joint 158 | :make-pivot-joint 159 | :ratchet-joint 160 | :make-ratchet-joint 161 | :rotary-limit-joint 162 | :make-rotary-limit-joint 163 | :simple-motor 164 | :make-simple-motor 165 | :slide-joint 166 | :make-slide-joint 167 | :spring 168 | :make-spring 169 | 170 | ;; callbacks 171 | :world-collision-callback 172 | :collide 173 | :defcollision 174 | 175 | ;; convenience 176 | :make-rectangle 177 | :make-circle-body 178 | :make-segment-body 179 | :make-poly-body 180 | :make-rectangle-body 181 | )) 182 | -------------------------------------------------------------------------------- /src/constraints/groove-joint.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defstruct (groove-joint (:include constraint) 5 | (:constructor 6 | make-groove-joint 7 | (body-a body-b groove-a groove-b anchor2 8 | &aux (groove-normal 9 | (vec-perp (vec-normalize (vec- groove-b groove-a))))))) 10 | groove-normal groove-a groove-b anchor2 11 | groove-transformed-normal clamp 12 | r1 r2 k1 k2 (j-acc +zero-vector+) j-max-length bias) 13 | 14 | (defmethod pre-step ((joint groove-joint) dt dt-inverse) 15 | (let* ((body-a (constraint-body-a joint)) 16 | (body-b (constraint-body-b joint)) 17 | (trans-a (body-local->world body-a (groove-joint-groove-a joint))) 18 | ;; The C source for this file uses body "a" for this one. Should it be body "b"? 19 | (trans-b (body-local->world body-a (groove-joint-groove-b joint))) 20 | ;; Calculate axis 21 | (normal (vec-rotate (groove-joint-groove-normal joint) 22 | (body-rotation body-a))) 23 | (d (vec. trans-a normal))) ; distance? dot product? What? 24 | (setf (groove-joint-groove-transformed-normal joint) normal 25 | (groove-joint-r2 joint) (vec-rotate (groove-joint-anchor2 joint) 26 | (body-rotation body-b))) 27 | ;; calculate tangential distance along the axis of r2 28 | (let ((td (vec-cross (vec+ (body-position body-b) (groove-joint-r2 joint)) normal))) 29 | ;; Calculate the clamping factor and r2 30 | (cond ((<= td (vec-cross trans-a normal)) 31 | (setf (groove-joint-clamp joint) 1d0 32 | (groove-joint-r1 joint) (vec- trans-a (body-position body-a)))) 33 | ((>= td (vec-cross trans-b normal)) 34 | (setf (groove-joint-clamp joint) -1d0 35 | (groove-joint-r1 joint) (vec- trans-b (body-position body-a)))) 36 | (t 37 | (setf (groove-joint-clamp joint) 0d0 38 | (groove-joint-r1 joint) (vec- (vec+ (vec* (vec-perp normal) (- td)) 39 | (vec* normal d)) 40 | (body-position body-a)))))) 41 | ;; calculate the mass tensor 42 | (multiple-value-bind (k1-val k2-val) 43 | (k-tensor body-a body-b (groove-joint-r1 joint) (groove-joint-r2 joint)) 44 | (setf (groove-joint-k1 joint) k1-val 45 | (groove-joint-k2 joint) k2-val)) 46 | 47 | ;; compute max impulse 48 | (setf (groove-joint-j-max-length joint) (impulse-max joint dt)) 49 | 50 | ;; Calculate bias velocity 51 | (let ((delta (vec- (vec+ (body-position body-b) (groove-joint-r2 joint)) 52 | (vec+ (body-position body-a) (groove-joint-r1 joint))))) 53 | (setf (groove-joint-bias joint) (vec-clamp (vec* delta 54 | (- (* (groove-joint-bias-coefficient joint) 55 | dt-inverse))) 56 | (groove-joint-max-bias joint)))) 57 | 58 | ;; Apply accumulated impulse 59 | (apply-impulses body-a body-b 60 | (groove-joint-r1 joint) (groove-joint-r2 joint) 61 | (groove-joint-j-acc joint)))) 62 | 63 | (defun constrain-groove (joint j) 64 | (let* ((normal (groove-joint-groove-transformed-normal joint)) 65 | (j-clamp (if (plusp (* (groove-joint-clamp joint) (vec-cross j normal))) 66 | j (vec-project j normal)))) 67 | (vec-clamp j-clamp (groove-joint-j-max-length joint)))) 68 | 69 | (defmethod apply-impulse ((joint groove-joint)) 70 | (let* ((body-a (groove-joint-body-a joint)) 71 | (body-b (groove-joint-body-b joint)) 72 | (r1 (groove-joint-r1 joint)) 73 | (r2 (groove-joint-r2 joint)) 74 | ;; Compute impulse... 75 | (relative-velocity (relative-velocity body-a body-b r1 r2)) 76 | (j (mult-k (vec- (groove-joint-bias joint) relative-velocity) 77 | (groove-joint-k1 joint) (groove-joint-k2 joint))) 78 | (j-old (groove-joint-j-acc joint))) 79 | (setf (groove-joint-j-acc joint) (constrain-groove joint (vec+ j-old j))) 80 | ;; Apply impulses 81 | (apply-impulses body-a body-b (groove-joint-r1 joint) (groove-joint-r2 joint) 82 | (vec- (groove-joint-j-acc joint) j-old)))) 83 | 84 | (defmethod get-impulse ((joint groove-joint)) 85 | (vec-length (groove-joint-j-acc joint))) 86 | -------------------------------------------------------------------------------- /src/hash-set.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (define-constant +primes+ 5 | (loop for x upfrom 2 6 | and offset in (list 1 3 1 5 3 3 1 9 7 5 3 17 27 3 1 29 7 | 3 21 7 17 15 9 43 35 15 29 3 -3) 8 | collect (+ (expt 2 x) offset))) 9 | 10 | ;;; I'm just sticking this bit here for now. Move it if you find a better place. 11 | (define-constant +chipmunk-hash-constant+ 3344921057) 12 | 13 | (declaim (ftype (function (fixnum fixnum) fixnum) hash-pair)) 14 | (defun hash-pair (x y) 15 | (logand (logxor (* x +chipmunk-hash-constant+) 16 | (* y +chipmunk-hash-constant+)) 17 | most-positive-fixnum)) 18 | 19 | (defun next-prime (n) 20 | (loop for prime in +primes+ when (>= prime n) return prime 21 | finally (error "Time to switch to native hashtables!"))) 22 | 23 | (defstruct (hash-set 24 | (:constructor 25 | make-hash-set (size test &aux 26 | (table (make-array (next-prime size) 27 | :initial-element nil))))) 28 | (count 0 :type fixnum) 29 | test 30 | (default-value nil) 31 | (table (assert nil) :type simple-vector)) 32 | 33 | (define-print-object (hash-set) 34 | (format t "Count: ~D" (hash-set-count hash-set))) 35 | 36 | (declaim (ftype (function (hash-set) fixnum) hash-set-size) 37 | (inline hash-set-size)) 38 | (defun hash-set-size (set) 39 | (length (hash-set-table set))) 40 | 41 | (declaim (ftype (function (hash-set fixnum) list) hash-set-chain)) 42 | (defun hash-set-chain (set index) 43 | (aref (hash-set-table set) index)) 44 | (defun (setf hash-set-chain) (new-chain set index) 45 | (setf (aref (hash-set-table set) index) new-chain)) 46 | 47 | (defun hash-set-full-p (set) 48 | (>= (hash-set-count set) 49 | (hash-set-size set))) 50 | 51 | (defun hash-set-resize (set &aux (new-size (next-prime (1+ (hash-set-size set))))) 52 | "Adjusts `hash-set' SET to accomodate more elements" 53 | (let ((new-table (make-array new-size :initial-element nil))) 54 | (loop for chain across (hash-set-table set) 55 | do (loop for bin in chain for index = (mod (car bin) new-size) 56 | do (push bin (aref new-table index)))) 57 | (setf (hash-set-table set) new-table) 58 | set)) 59 | 60 | (defun hash-set-insert (set code data) 61 | "Insert DATA into `hash-set' SET, using hash value CODE. Returns DATA if an 62 | insertion was made, or NIL if DATA was already present in the table." 63 | (let ((index (mod code (hash-set-size set)))) 64 | (with-accessors ((test hash-set-test)) set 65 | (unless (find data (hash-set-chain set index) :test test :key #'cdr) 66 | (when (hash-set-full-p set) 67 | (hash-set-resize set)) 68 | (push (cons code data) (hash-set-chain set index)) 69 | (incf (hash-set-count set)) 70 | data)))) 71 | 72 | (defun hash-set-find (set code data) 73 | "Searches for DATA in `hash-set' SET, using hash value CODE. On success, two 74 | values are returned: the datum found within SET, and T. On failure, the values 75 | are the `hash-set-default-value' for SET, and NIL. See `cl:gethash'." 76 | (let ((chain (hash-set-chain set (mod code (hash-set-size set))))) 77 | (dolist (bin chain (values (hash-set-default-value set) nil)) 78 | (when (funcall (hash-set-test set) data (cdr bin)) 79 | (return (values (cdr bin) t)))))) 80 | 81 | (defun hash-set-find-if (predicate set code) 82 | (let ((chain (hash-set-chain set (mod code (hash-set-size set))))) 83 | (dolist (bin chain (values (hash-set-default-value set) nil)) 84 | (when (funcall predicate (cdr bin)) 85 | (return (values (cdr bin) t)))))) 86 | 87 | (defun hash-set-remove (set code data) 88 | "Removes DATA from `hash-set' SET, using hash value CODE. On success, two 89 | values are returned: the datum removed from SET, and T. On failure, the values 90 | are the `hash-set-default-value' for SET, and NIL. See `cl:remhash'." 91 | (multiple-value-bind (datum found) (hash-set-find set code data) 92 | (when found 93 | (decf (hash-set-count set)) 94 | (let ((index (mod code (hash-set-size set)))) 95 | (deletef (hash-set-chain set index) 96 | datum :test #'eq :key #'cdr))) 97 | (values datum found))) 98 | 99 | (defun map-hash-set (function set) 100 | "Calls FUNCTION once on each datum in the `hash-set' SET, and returns NIL." 101 | (loop for chain across (hash-set-table set) 102 | do (dolist (bin chain) 103 | (funcall function (cdr bin))))) 104 | 105 | (defun hash-set-delete-if (predicate set) 106 | "Deletes the items from `hash-set' SET on which PREDICATE is true. Returns NIL." 107 | (dotimes (index (hash-set-size set)) 108 | (when (hash-set-chain set index) 109 | (let ((before (length (hash-set-chain set index)))) 110 | (delete-iff (hash-set-chain set index) predicate :key #'cdr) 111 | (decf (hash-set-count set) (- before (length (hash-set-chain set index)))))))) 112 | -------------------------------------------------------------------------------- /demo/logo-smash.lisp: -------------------------------------------------------------------------------- 1 | (in-package :squirl-demo) 2 | 3 | (defparameter *logo* #(15 -16 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 7 -64 15 63 -32 -2 0 0 0 0 0 0 0 4 | 0 0 0 0 0 0 0 0 0 0 0 31 -64 15 127 -125 -1 -128 0 0 0 0 0 0 0 0 0 0 0 0 0 0 5 | 0 0 0 127 -64 15 127 15 -1 -64 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 -1 -64 15 -2 6 | 31 -1 -64 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 -1 -64 0 -4 63 -1 -32 0 0 0 0 0 0 7 | 0 0 0 0 0 0 0 0 0 0 1 -1 -64 15 -8 127 -1 -32 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 8 | 1 -1 -64 0 -8 -15 -1 -32 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 -31 -1 -64 15 -8 -32 9 | -1 -32 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 7 -15 -1 -64 9 -15 -32 -1 -32 0 0 0 0 0 10 | 0 0 0 0 0 0 0 0 0 0 31 -15 -1 -64 0 -15 -32 -1 -32 0 0 0 0 0 0 0 0 0 0 0 0 0 11 | 0 0 63 -7 -1 -64 9 -29 -32 127 -61 -16 63 15 -61 -1 -8 31 -16 15 -8 126 7 -31 12 | -8 31 -65 -7 -1 -64 9 -29 -32 0 7 -8 127 -97 -25 -1 -2 63 -8 31 -4 -1 15 -13 13 | -4 63 -1 -3 -1 -64 9 -29 -32 0 7 -8 127 -97 -25 -1 -2 63 -8 31 -4 -1 15 -13 14 | -2 63 -1 -3 -1 -64 9 -29 -32 0 7 -8 127 -97 -25 -1 -1 63 -4 63 -4 -1 15 -13 15 | -2 63 -33 -1 -1 -32 9 -25 -32 0 7 -8 127 -97 -25 -1 -1 63 -4 63 -4 -1 15 -13 16 | -1 63 -33 -1 -1 -16 9 -25 -32 0 7 -8 127 -97 -25 -1 -1 63 -4 63 -4 -1 15 -13 17 | -1 63 -49 -1 -1 -8 9 -57 -32 0 7 -8 127 -97 -25 -8 -1 63 -2 127 -4 -1 15 -13 18 | -1 -65 -49 -1 -1 -4 9 -57 -32 0 7 -8 127 -97 -25 -8 -1 63 -2 127 -4 -1 15 -13 19 | -1 -65 -57 -1 -1 -2 9 -57 -32 0 7 -8 127 -97 -25 -8 -1 63 -2 127 -4 -1 15 -13 20 | -1 -1 -57 -1 -1 -1 9 -57 -32 0 7 -1 -1 -97 -25 -8 -1 63 -1 -1 -4 -1 15 -13 -1 21 | -1 -61 -1 -1 -1 -119 -57 -32 0 7 -1 -1 -97 -25 -8 -1 63 -1 -1 -4 -1 15 -13 -1 22 | -1 -61 -1 -1 -1 -55 -49 -32 0 7 -1 -1 -97 -25 -8 -1 63 -1 -1 -4 -1 15 -13 -1 23 | -1 -63 -1 -1 -1 -23 -49 -32 127 -57 -1 -1 -97 -25 -1 -1 63 -1 -1 -4 -1 15 -13 24 | -1 -1 -63 -1 -1 -1 -16 -49 -32 -1 -25 -1 -1 -97 -25 -1 -1 63 -33 -5 -4 -1 15 25 | -13 -1 -1 -64 -1 -9 -1 -7 -49 -32 -1 -25 -8 127 -97 -25 -1 -1 63 -33 -5 -4 -1 26 | 15 -13 -1 -1 -64 -1 -13 -1 -32 -49 -32 -1 -25 -8 127 -97 -25 -1 -2 63 -49 -13 27 | -4 -1 15 -13 -1 -1 -64 127 -7 -1 -119 -17 -15 -1 -25 -8 127 -97 -25 -1 -2 63 28 | -49 -13 -4 -1 15 -13 -3 -1 -64 127 -8 -2 15 -17 -1 -1 -25 -8 127 -97 -25 -1 29 | -8 63 -49 -13 -4 -1 15 -13 -3 -1 -64 63 -4 120 0 -17 -1 -1 -25 -8 127 -97 -25 30 | -8 0 63 -57 -29 -4 -1 15 -13 -4 -1 -64 63 -4 0 15 -17 -1 -1 -25 -8 127 -97 31 | -25 -8 0 63 -57 -29 -4 -1 -1 -13 -4 -1 -64 31 -2 0 0 103 -1 -1 -57 -8 127 -97 32 | -25 -8 0 63 -57 -29 -4 -1 -1 -13 -4 127 -64 31 -2 0 15 103 -1 -1 -57 -8 127 33 | -97 -25 -8 0 63 -61 -61 -4 127 -1 -29 -4 127 -64 15 -8 0 0 55 -1 -1 -121 -8 34 | 127 -97 -25 -8 0 63 -61 -61 -4 127 -1 -29 -4 63 -64 15 -32 0 0 23 -1 -2 3 -16 35 | 63 15 -61 -16 0 31 -127 -127 -8 31 -1 -127 -8 31 -128 7 -128 0 0)) 36 | 37 | (defparameter *image-width* 188) 38 | (defparameter *image-height* 35) 39 | (defparameter *image-row-length* 24) 40 | (defvar *bullet*) 41 | 42 | (defclass logo-smash (demo) 43 | () 44 | (:default-initargs :name "Smash that damn logo." :physics-timestep 1/60 45 | :draw-shapes-p nil :draw-bb-p nil :body-point-size 3 :collision-point-size 0)) 46 | 47 | (defun get-pixel (x y) 48 | (oddp (ash (svref *logo* (+ (ash x -3) (* y *image-row-length*))) 49 | (- (mod x 8) 7)))) 50 | 51 | (defun make-ball (x y) 52 | (make-body :mass 1.0 :position (vec x y) :shapes (list (make-circle 0.95)))) 53 | 54 | (defmethod update-demo ((demo logo-smash) dt) 55 | (declare (ignore dt)) 56 | (world-step (world demo) (physics-timestep demo))) 57 | 58 | (defmethod init-demo ((demo logo-smash)) 59 | (setf (world demo) (make-world :iterations 1)) 60 | (resize-world-active-hash (world demo) 2.0 10000) 61 | (loop for y below *image-height* 62 | do (loop for x below *image-width* 63 | for x-jitter = (random 0.05) for y-jitter = (random 0.05) 64 | when (get-pixel x y) 65 | do (let ((ball (make-ball (* 2 (- x (/ *image-width* 2) (- x-jitter))) 66 | (* 2 (- (/ *image-height* 2) y (- y-jitter)))))) 67 | (world-add-body (world demo) ball)))) 68 | (let ((bullet (make-body :position (vec -800 -10) 69 | :velocity (vec 400 0) 70 | :mass 100000 :inertia 100000 71 | :actor :not-grabbable))) 72 | (attach-shape (make-circle 8) bullet) 73 | (world-add-body (world demo) bullet) 74 | (setf *bullet* bullet)) 75 | (world demo)) 76 | 77 | (pushnew 'logo-smash *demos*) 78 | -------------------------------------------------------------------------------- /demo/theo-jansen.lisp: -------------------------------------------------------------------------------- 1 | (in-package :squirl-demo) 2 | 3 | (defclass theo-jansen (demo) 4 | ((static-body :accessor static-body) 5 | (walker :accessor demo-walker) 6 | (motor :accessor demo-motor)) 7 | (:default-initargs :name "Theo Jansen Walker")) 8 | 9 | (defparameter *seg-radius* 3) 10 | 11 | (defmethod update-demo :before ((demo theo-jansen) dt) 12 | (declare (ignore dt)) 13 | (let* ((coef (/ (+ 2 (vec-y *arrow-direction*)) 3)) 14 | (rate (* (vec-x *arrow-direction*) 10 coef))) 15 | (setf (squirl::simple-motor-rate (demo-motor demo)) rate 16 | (squirl::simple-motor-max-force (demo-motor demo)) (if (zerop rate) 0d0 100000)))) 17 | 18 | (defclass walker () 19 | ((chassis :initarg :chassis :accessor walker-chassis) 20 | (crank :initarg :crank :accessor walker-crank) 21 | (upper-legs :initform nil :initarg :upper-legs :accessor walker-upper-legs) 22 | (lower-legs :initform nil :initarg :lower-legs :accessor walker-lower-legs))) 23 | 24 | (defcollision ((a walker) (b walker) contacts) (not (eq a b))) 25 | (defcollision ((a walker) (b (eql :not-grabbable)) contacts) (declare (ignore a b contacts)) t) 26 | 27 | (defun make-leg (world side offset walker anchor &aux (leg-mass 1)) 28 | (let* ((upper-leg (world-add-body 29 | world 30 | (make-body :mass leg-mass :position (vec offset 0) :actor walker 31 | :shapes (list 32 | (make-segment +zero-vector+ (vec 0 side) 33 | :radius *seg-radius*))))) 34 | (lower-leg (world-add-body 35 | world 36 | (make-body :mass leg-mass :position (vec offset 0) :actor walker 37 | :shapes (list 38 | (make-segment +zero-vector+ (vec 0 (- side)) 39 | :radius *seg-radius*) 40 | (make-circle (* 2 *seg-radius*) 41 | :center (vec 0 (- side)) 42 | :friction 1))))) 43 | (diag (sqrt (+ (* side side) (* offset offset)))) 44 | (chassis (walker-chassis walker)) 45 | (crank (walker-crank walker))) 46 | (pushnew upper-leg (walker-upper-legs walker)) 47 | (pushnew lower-leg (walker-lower-legs walker)) 48 | ;; add constraints 49 | (world-add-constraint world (make-pivot-joint chassis upper-leg (vec offset 0) +zero-vector+)) 50 | (world-add-constraint world (make-pin-joint chassis lower-leg (vec offset 0) +zero-vector+)) 51 | (world-add-constraint world (make-gear-joint upper-leg lower-leg 0 1)) 52 | (let ((joint (make-pin-joint crank upper-leg anchor (vec 0 side)))) 53 | (setf (squirl::pin-joint-distance joint) diag) 54 | (world-add-constraint world joint)) 55 | (let ((joint (make-pin-joint crank lower-leg anchor +zero-vector+))) 56 | (setf (squirl::pin-joint-distance joint) diag) 57 | (world-add-constraint world joint)))) 58 | 59 | (defmethod init-demo ((demo theo-jansen)) 60 | (setf (demo-walker demo) (make-instance 'walker) 61 | (world demo) (make-world :iterations 20 :gravity (vec 0 -500)) 62 | (static-body demo) (world-add-body 63 | (world demo) 64 | (make-body :actor :not-grabbable 65 | :shapes 66 | (list (make-segment (vec -320 -240) (vec -320 240) 67 | :restitution 1 :friction 1) 68 | (make-segment (vec 320 -240) (vec 320 240) 69 | :restitution 1 :friction 1) 70 | (make-segment (vec -320 -240) (vec 320 -240) 71 | :restitution 1 :friction 1))))) 72 | (let ((offset 30) (chassis-mass 2) (crank-mass 1) (crank-radius 13d0) (side 30) (num-legs 2)) 73 | (setf (walker-chassis (demo-walker demo)) 74 | (world-add-body (world demo) 75 | (make-body :mass chassis-mass :actor (demo-walker demo) 76 | :shapes (list (make-segment (vec (- offset) 0) 77 | (vec offset 0) 78 | :radius *seg-radius*)))) 79 | (walker-crank (demo-walker demo)) 80 | (world-add-body (world demo) 81 | (make-body :mass crank-mass :actor (demo-walker demo) 82 | :shapes (list (make-circle crank-radius))))) 83 | (world-add-constraint (world demo) 84 | (make-pivot-joint (walker-chassis (demo-walker demo)) 85 | (walker-crank (demo-walker demo)) 86 | +zero-vector+ +zero-vector+)) 87 | ;; add the legs... 88 | (loop for i below num-legs do 89 | (make-leg (world demo) side offset (demo-walker demo) 90 | (vec* (angle->vec (/ (* 2 i) (* num-legs pi))) crank-radius)) 91 | (make-leg (world demo) side (- offset) (demo-walker demo) 92 | (vec* (angle->vec (/ (* 2 (1+ i)) (* num-legs pi))) crank-radius))) 93 | (setf (demo-motor demo) 94 | (world-add-constraint (world demo) 95 | (make-simple-motor (walker-chassis (demo-walker demo)) 96 | (walker-crank (demo-walker demo)) 6))) 97 | (world demo))) 98 | 99 | (pushnew 'theo-jansen *demos*) 100 | -------------------------------------------------------------------------------- /demo/pump.lisp: -------------------------------------------------------------------------------- 1 | (in-package :squirl-demo) 2 | 3 | (defclass pump-demo (demo) 4 | ((static-body :accessor demo-static-body) 5 | (balls :initform nil :accessor demo-balls)) 6 | (:default-initargs :name "Pump it up!")) 7 | 8 | (defvar *static-body*) 9 | (defvar *motor*) 10 | (defparameter *num-balls* 4) 11 | 12 | (defmethod update-demo ((demo pump-demo) dt) 13 | (declare (ignore dt)) 14 | (let* ((coef (/ (+ 2 (vec-y *arrow-direction*)) 3)) 15 | (rate (* (vec-x *arrow-direction*) 30 coef))) 16 | (setf (squirl::simple-motor-rate *motor*) rate 17 | (squirl::simple-motor-max-force *motor*) (if (zerop rate) 0 1000000)) 18 | (sleep 0.016) 19 | (world-step (world demo) (physics-timestep demo)) 20 | (loop for ball in (demo-balls demo) 21 | do (when (> (vec-x (body-position ball)) 320) 22 | (setf (body-velocity ball) +zero-vector+) 23 | (setf (body-position ball) (vec -224 200)))))) 24 | 25 | (defclass ball () ()) 26 | (defclass feeder () ()) 27 | (defclass plunger () ()) 28 | (defclass gear () ()) 29 | 30 | ;; balls collide with everything 31 | (defcollision ((ball ball) anything contacts) t) 32 | ;; the plunger collides with the hopper, but not with the gears 33 | (defcollision ((a plunger) (hopper (eql :not-grabbable)) contacts) t) 34 | (defcollision ((a plunger) (b gear) contacts) nil) 35 | ;; the feeder collides with the balls, but not with anything else 36 | (defcollision ((a feeder) anything contacts) nil) 37 | (defcollision ((feeder feeder) (ball ball) contacts) t) 38 | ;; gears collide with each other, though 39 | (defcollision ((a gear) (another gear) contacts) t) 40 | 41 | (defun add-ball (world pos) 42 | (world-add-body world (make-body :mass 1 :position pos :actor (make-instance 'ball) 43 | :shapes (list (make-circle 30 :friction 0.5))))) 44 | 45 | (defun setup-static-body (world) 46 | (world-add-body world 47 | (make-body :actor :not-grabbable 48 | :shapes (list (make-segment (vec -256 16) (vec -256 240) :radius 2 49 | :restitution 1 :friction 0.5) 50 | (make-segment (vec -256 16) (vec -192 0) :radius 2 51 | :restitution 1 :friction 0.5) 52 | (make-segment (vec -192 0) (vec -192 -64) :radius 2 53 | :restitution 1 :friction 0.5) 54 | (make-segment (vec -128 -64) (vec -128 144) :radius 2 55 | :restitution 1 :friction 0.5) 56 | (make-segment (vec -192 80) (vec -192 176) :radius 2 57 | :restitution 1 :friction 0.5) 58 | (make-segment (vec -192 176) (vec -128 240) :radius 2 59 | :restitution 1 :friction 0.5) 60 | (make-segment (vec -128 144) (vec 192 64) :radius 2 61 | :restitution 1 :friction 0.5))))) 62 | (defun add-plunger (world) 63 | (let ((verts (list (vec -30 -80) 64 | (vec -30 80) 65 | (vec 30 64) 66 | (vec 30 -80)))) 67 | (world-add-body 68 | world (make-body :mass 1 :position (vec -160 -80) :actor (make-instance 'plunger) 69 | :shapes (list (make-poly verts :restitution 1 :friction 0.5)))))) 70 | 71 | (defun add-small-gear (world static-body) 72 | (let ((gear (world-add-body 73 | world (make-body :mass 10 :angle (/ pi -2) :actor (make-instance 'gear) 74 | :position (vec -160 -160) :shapes (list (make-circle 80)))))) 75 | (world-add-constraint world (make-pivot-joint static-body gear (vec -160 -160) +zero-vector+)) 76 | gear)) 77 | 78 | (defun add-big-gear (world static-body) 79 | (let ((gear (world-add-body 80 | world (make-body :mass 40 :actor (make-instance 'gear) 81 | :position (vec 80 -160) :angle (/ pi 2) 82 | :shapes (list (make-circle 160)))))) 83 | (world-add-constraint world (make-pivot-joint static-body gear (vec 80 -160) +zero-vector+)) 84 | gear)) 85 | 86 | (defun add-constraints (world small-gear plunger big-gear) 87 | (world-add-constraint world (make-pin-joint small-gear plunger (vec 80 0) +zero-vector+)) 88 | (world-add-constraint world (make-gear-joint small-gear big-gear (/ pi 2) -2))) 89 | 90 | (defun add-feeder (world static-body small-gear) 91 | (let* ((bottom -300) (top 32) (length (- top bottom)) 92 | (feeder (make-body :mass 1 :actor (make-instance 'feeder) 93 | :position (vec -224 (/ (+ bottom top) 2)) 94 | :shapes (list (make-segment (vec 0 (/ length 2)) (vec 0 (- (/ length 2))) 95 | :radius 20))))) 96 | (world-add-body world feeder) 97 | (world-add-constraint world (make-pivot-joint static-body feeder 98 | (vec -224 bottom) (vec 0 (- (/ length 2))))) 99 | (world-add-constraint world (make-pin-joint feeder small-gear 100 | (world->body-local feeder (vec -224 -160)) 101 | (vec 0 80))))) 102 | 103 | (defun motorize-gear (world static-body big-gear) 104 | (setf *motor* (world-add-constraint world (make-simple-motor static-body big-gear 3)))) 105 | 106 | (defmethod init-demo ((demo pump-demo)) 107 | (setf (world demo) (make-world :gravity (vec 0 -600))) 108 | (setf (demo-static-body demo) (setup-static-body (world demo))) 109 | (dotimes (i *num-balls*) 110 | (pushnew (add-ball (world demo) (vec -224 (+ 80 (* i 64)))) 111 | (demo-balls demo))) 112 | (let* ((plunger (add-plunger (world demo))) 113 | (small-gear (add-small-gear (world demo) (demo-static-body demo))) 114 | (big-gear (add-big-gear (world demo) (demo-static-body demo)))) 115 | (add-constraints (world demo) small-gear plunger big-gear) 116 | (add-feeder (world demo) (demo-static-body demo) small-gear) 117 | (motorize-gear (world demo) (demo-static-body demo) big-gear)) 118 | (world demo)) 119 | 120 | (pushnew 'pump-demo *demos*) 121 | -------------------------------------------------------------------------------- /src/utils.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl.utils) 3 | 4 | (locally (declare (optimize speed)) 5 | 6 | (declaim (inline make-adjustable-vector clamp ensure-list) 7 | (ftype (function (fixnum) vector) make-adjustable-vector) 8 | (ftype (function (double-float double-float double-float) double-float)) 9 | (ftype (function (t) list) ensure-list)) 10 | 11 | (defun make-adjustable-vector (length) 12 | (make-array length :adjustable t :fill-pointer 0)) 13 | 14 | (defun ensure-list (x) (if (listp x) x (list x))) 15 | 16 | (defun clamp (n min max) 17 | (declare (double-float n min max)) 18 | (min (max n min) max)) 19 | 20 | ) ; LOCALLY 21 | 22 | (declaim (inline maybe/) 23 | (ftype (function (double-float &optional double-float) double-float) maybe/)) 24 | (defun maybe/ (a &optional b) 25 | ;; Don't declare me (optimize speed), because that chokes SBCL 26 | (if (zerop b) 0d0 (/ a b))) 27 | 28 | (defun maybe-inverse (x) 29 | (if (zerop x) 0d0 (/ x))) 30 | 31 | (defmacro fun (&body body) 32 | `(lambda (&optional _) (declare (ignorable _)) ,@body)) 33 | 34 | ;; from alexandria: 35 | (declaim (inline delete/swapped-arguments delete-if/swapped-arguments)) 36 | (defun delete/swapped-arguments (sequence item &rest keyword-arguments) 37 | (apply #'delete item sequence keyword-arguments)) 38 | (defun delete-if/swapped-arguments (sequence predicate &rest keyword-arguments) 39 | (apply #'delete-if predicate sequence keyword-arguments)) 40 | 41 | (define-modify-macro deletef (item &rest remove-keywords) 42 | delete/swapped-arguments 43 | "Modify-macro for DELETE. Sets place designated by the first argument to 44 | the result of calling DELETE with ITEM, place, and the REMOVE-KEYWORDS.") 45 | 46 | (define-modify-macro delete-iff (predicate &rest remove-keywords) 47 | delete-if/swapped-arguments 48 | "Modify-macro for DELETE-IF. Sets place designated by the first argument to 49 | the result of calling DELETE with PREDICATE, place, and the REMOVE-KEYWORDS.") 50 | 51 | (defmacro with-gensyms ((&rest vars) &body body) 52 | `(let ,(loop for var in vars collect `(,var (gensym ,(symbol-name var)))) 53 | ,@body)) 54 | 55 | (defmacro without-floating-point-underflow (form) 56 | #+clisp `(ext:without-floating-point-underflow ,form) 57 | form) 58 | 59 | (eval-when (:compile-toplevel :load-toplevel :execute) 60 | (defun symbolicate (&rest things) 61 | "Concatenate together the names of some strings and symbols, 62 | producing a symbol in the current package." 63 | (let ((name (make-string (reduce #'+ things :key (fun (length (string _))))))) 64 | (let ((index 0)) 65 | (dolist (thing things (values (intern name))) 66 | (let ((x (string thing))) 67 | (replace name x :start1 index) 68 | (incf index (length x)))))))) 69 | 70 | (macrolet ((define-ensure-foo (place) ; Lisp macros are nice 71 | `(defun ,(symbolicate "ENSURE-" place) (place &optional (default place)) 72 | (if (atom place) default (,place place))))) 73 | (define-ensure-foo car) 74 | (define-ensure-foo cadr)) 75 | 76 | (defmacro push-cons (cons place) 77 | "Like `cl:push', but reuses CONS" 78 | (with-gensyms (cons-sym) 79 | `(let ((,cons-sym ,cons)) 80 | (setf (cdr ,cons-sym) ,place 81 | ,place ,cons-sym)))) 82 | 83 | (defmacro define-constant (name value &optional doc) 84 | "ANSI-compliant replacement for `defconstant'. cf SBCL Manual 2.3.4." 85 | `(defconstant ,name (if (boundp ',name) (symbol-value ',name) ,value) 86 | ,@(when doc (list doc)))) 87 | 88 | (defmacro define-print-object ((class &key (identity t) (type t)) &body body) 89 | "Defines a `print-object' method on class CLASS, using the standard macro 90 | `print-unreadable-object'. The IDENTITY and TYPE keyword arguments are passed 91 | through to `print-unreadable-object', although they default to T if not supplied. 92 | 93 | CLASS can be a list of the form (VARIABLE CLASS-NAME), in which case 94 | the `print-object' method will be specialized on class CLASS-NAME and VARIABLE 95 | will be used as the parameter name. Alternatively, as shorthand, CLASS can be a 96 | single symbol, which will be used for both the variable and the class name." 97 | (let ((object (ensure-car class)) 98 | (class-name (ensure-cadr class))) 99 | (with-gensyms (stream) 100 | `(defmethod print-object ((,object ,class-name) ,stream) 101 | (print-unreadable-object (,object ,stream :type ,type :identity ,identity) 102 | (let ((*standard-output* ,stream)) ,@body)))))) 103 | 104 | (defmacro do-vector ((var vector-form &optional result) &body body) 105 | "See `dolist'. If VAR is a list of the form (INDEX VAR), then INDEX is used 106 | as the index vector. Note that this macro doesn't handle declarations properly." 107 | (let ((var-name (ensure-cadr var)) 108 | (idx-name (ensure-car var (gensym "INDEX")))) 109 | (with-gensyms (vector) 110 | `(let ((,vector ,vector-form) ,var-name) 111 | (declare (ignorable ,var-name) (vector ,vector)) 112 | (dotimes (,idx-name (length ,vector) ,result) 113 | (let ((,var-name (aref ,vector ,idx-name))) ,@body)))))) 114 | 115 | (defmacro with-place (conc-name (&rest slots) form &body body) 116 | (let* ((sm-prefix (ensure-car conc-name)) 117 | (acc-prefix (ensure-cadr conc-name)) 118 | (*package* (symbol-package sm-prefix))) 119 | `(with-accessors 120 | ,(mapcar (fun (list (symbolicate sm-prefix (ensure-car _)) 121 | (symbolicate acc-prefix (ensure-cadr _)))) 122 | slots) 123 | ,form 124 | ,@body))) 125 | 126 | (defmacro aprog1 (result &body body) 127 | `(let ((it ,result)) ,@body it)) 128 | 129 | (defmacro aif (test-form then-form &optional else-form) 130 | `(let ((it ,test-form)) 131 | (if it ,then-form ,else-form))) 132 | 133 | (defmacro awhen (test-form &body body) 134 | `(aif ,test-form 135 | (progn ,@body))) 136 | 137 | (defun parse-defmethod (args) 138 | (let (qualifiers lambda-list body (parse-state :qualifiers)) 139 | (dolist (arg args) 140 | (ecase parse-state 141 | (:qualifiers (if (and (atom arg) 142 | (not (null arg))) 143 | (push arg qualifiers) 144 | (setf lambda-list arg 145 | parse-state :body))) 146 | (:body (push arg body)))) 147 | (values qualifiers lambda-list (nreverse body)))) 148 | 149 | (defmacro pop-declarations (place) 150 | "Returns and removes all leading declarations from PLACE, which should be 151 | a setf-able form. NOTE: This is a kludge hack shit substitute for parse-declarations" 152 | (with-gensyms (form) 153 | `(loop for ,form in ,place 154 | while (handler-case (string-equal (car ,form) 'declare) (type-error ())) 155 | collect (pop ,place)))) 156 | -------------------------------------------------------------------------------- /demo/springies.lisp: -------------------------------------------------------------------------------- 1 | (in-package :squirl-demo) 2 | 3 | (defclass springies-demo (demo) 4 | ((static-body :initarg :box :accessor static-body)) 5 | (:default-initargs :name "Sproing twang!" :physics-timestep (float 1/60 1d0))) 6 | 7 | (defstruct (springy-spring (:include damped-spring) 8 | (:constructor 9 | make-springy-spring 10 | (body-a body-b anchor1 anchor2 rest-length stiffness damping)))) 11 | 12 | (defmethod squirl::spring-force (spring distance &aux (clamp 20d0)) 13 | (* (squirl::clamp (- (squirl::damped-spring-rest-length spring) distance) (- clamp) clamp) 14 | (squirl::damped-spring-stiffness spring))) 15 | 16 | (defclass springy () 17 | ((bodies :initarg :bodies :initform nil :accessor springy-bodies))) 18 | 19 | (defmethod initialize-instance :after ((springy springy) &key) 20 | (dolist (body (springy-bodies springy)) 21 | (setf (body-actor body) springy))) 22 | 23 | (defcollision ((a springy) (b springy) contacts) (declare (ignore contacts)) (unless (eq a b) t)) 24 | 25 | (defun build-springies (world) 26 | (let ((springies 27 | (list (make-instance 'springy :bodies (list (add-bar world (vec -240 160) (vec -160 80)) 28 | (add-bar world (vec -160 80) (vec -80 160)))) 29 | (make-instance 'springy :bodies (list (add-bar world (vec 0 160) (vec 80 0)))) 30 | (make-instance 'springy :bodies (list (add-bar world (vec 160 160) (vec 240 160)))) 31 | (make-instance 'springy :bodies (list (add-bar world (vec -240 0) (vec -160 -80)) 32 | (add-bar world (vec -160 -80) (vec -80 0)) 33 | (add-bar world (vec -80 0) (vec 0 0)))) 34 | (make-instance 'springy :bodies (list (add-bar world (vec 0 -80) (vec 80 -80)))) 35 | (make-instance 'springy :bodies (list (add-bar world (vec 240 80) (vec 160 0)) 36 | (add-bar world (vec 160 0) (vec 240 -80)))) 37 | (make-instance 'springy :bodies (list (add-bar world (vec -240 -80) (vec -160 -160)) 38 | (add-bar world (vec -160 -160) (vec -80 -160)))) 39 | (make-instance 'springy :bodies (list (add-bar world (vec 0 -160) (vec 80 -160)))) 40 | (make-instance 'springy :bodies (list (add-bar world (vec 160 -160) (vec 240 -160))))))) 41 | (world-add-constraint world (make-pivot-joint (first (springy-bodies (first springies))) 42 | (second (springy-bodies (first springies))) 43 | (vec 40 -40) (vec -40 -40))) 44 | (world-add-constraint world (make-pivot-joint (first (springy-bodies (fourth springies))) 45 | (second (springy-bodies (fourth springies))) 46 | (vec 40 -40) (vec -40 -40))) 47 | (world-add-constraint world (make-pivot-joint (second (springy-bodies (fourth springies))) 48 | (third (springy-bodies (fourth springies))) 49 | (vec 40 40) (vec -40 0))) 50 | (world-add-constraint world (make-pivot-joint (first (springy-bodies (sixth springies))) 51 | (second (springy-bodies (sixth springies))) 52 | (vec -40 -40) (vec -40 40))) 53 | (world-add-constraint world (make-pivot-joint (first (springy-bodies (seventh springies))) 54 | (second (springy-bodies (seventh springies))) 55 | (vec 40 -40) (vec -40 0))) 56 | springies)) 57 | 58 | (defun add-bar (world point-a point-b) 59 | (let* ((center (vec* (vec+ point-a point-b) 0.5d0)) 60 | (length (vec-length (vec- point-a point-b))) 61 | (mass (/ length 160))) 62 | (world-add-body world 63 | (make-body :mass mass :inertia (* mass length (/ length 12)) :position center 64 | :shapes (list (make-segment (vec- point-a center) (vec- point-b center) 65 | :radius 10)))))) 66 | 67 | (defun add-springs (world static-body springies &aux (sb static-body)) 68 | (let ((bodies (mapcan (lambda (_) (copy-list (springy-bodies _))) springies)) 69 | (stiffness 100) (damping 0.5)) 70 | (flet ((con (body-a-or-n body-b anchor1-x anchor1-y anchor2-x anchor2-y) 71 | (world-add-constraint world (make-springy-spring 72 | (if (numberp body-a-or-n) 73 | (elt bodies (1- body-a-or-n)) 74 | body-a-or-n) 75 | (elt bodies (1- body-b)) 76 | (vec anchor1-x anchor1-y) 77 | (vec anchor2-x anchor2-y) 78 | 0 stiffness damping)))) 79 | ;; against static body 80 | (con sb 1 -320 240 -40 40) 81 | (con sb 1 -320 80 -40 40) 82 | (con sb 1 -160 240 -40 40) 83 | (con sb 2 -160 240 40 40) 84 | (con sb 2 0 240 40 40) 85 | (con sb 3 80 240 -40 80) 86 | (con sb 4 80 240 -40 0) 87 | (con sb 4 320 240 40 0) 88 | (con sb 5 -320 80 -40 40) 89 | (con sb 9 320 80 40 40) 90 | (con sb 10 320 0 40 -40) 91 | (con sb 10 320 -160 40 -40) 92 | (con sb 11 -320 -160 -40 40) 93 | (con sb 12 -240 -240 -40 0) 94 | (con sb 12 0 -240 40 0) 95 | (con sb 13 0 -240 -40 0) 96 | (con sb 13 80 -240 40 0) 97 | (con sb 14 80 -240 -40 0) 98 | (con sb 14 240 -240 40 0) 99 | (con sb 14 320 -160 40 0) 100 | ;; springy/springy 101 | (con 1 5 40 -40 -40 40) 102 | (con 1 6 40 -40 40 40) 103 | (con 2 3 40 40 -40 80) 104 | (con 3 4 -40 80 -40 0) 105 | (con 3 4 40 -80 -40 0) 106 | (con 3 7 40 -80 40 0) 107 | (con 3 7 -40 80 -40 0) 108 | (con 3 8 40 -80 40 0) 109 | (con 3 9 40 -80 -40 -40) 110 | (con 4 9 40 0 40 40) 111 | (con 5 11 -40 40 -40 40) 112 | (con 5 11 40 -40 40 -40) 113 | (con 7 8 40 0 -40 0) 114 | (con 8 12 -40 0 40 0) 115 | (con 8 13 -40 0 -40 0) 116 | (con 8 13 40 0 40 0) 117 | (con 8 14 40 0 -40 0) 118 | (con 10 14 40 -40 -40 0) 119 | (con 10 14 40 -40 -40 0)))) 120 | 121 | (defmethod init-demo ((demo springies-demo)) 122 | (setf (static-body demo) (make-body) 123 | (world demo) (make-world)) 124 | (resize-world-active-hash (world demo) 30 999) 125 | (resize-world-static-hash (world demo) 200 99) 126 | (add-springs (world demo) (static-body demo) (build-springies (world demo))) 127 | (world demo)) 128 | 129 | (pushnew 'springies-demo *demos*) 130 | -------------------------------------------------------------------------------- /src/poly-shape.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (declaim (inline make-poly-axis)) 5 | (defstruct (poly-axis 6 | (:constructor make-poly-axis (normal distance))) 7 | (normal +zero-vector+ :type vec) 8 | (distance 0d0 :type double-float)) 9 | 10 | (defstruct (poly (:include shape) 11 | (:constructor 12 | %make-poly (length restitution friction &aux 13 | (transformed-vertices (make-array length :element-type 'vec)) 14 | (transformed-axes (make-array length :element-type 'poly-axis))))) 15 | (vertices (make-array 0 :element-type 'vec) :type (simple-array vec (*))) 16 | (axes (make-array 0 :element-type 'poly-axis) :type (simple-array poly-axis (*))) 17 | (transformed-vertices (assert nil) :type (simple-array vec (*))) 18 | (transformed-axes (assert nil) :type (simple-array poly-axis (*)))) 19 | 20 | (defmethod print-shape progn ((poly poly)) 21 | (format t "Vertex count: ~a" (length (poly-vertices poly)))) 22 | 23 | (defun compute-new-vertices (vertices offset &aux (limit (1- (length vertices)))) 24 | ;; The proper approach here would be to maintain a separate "fill pointer" aka index 25 | ;; Three advantages -- we need not return new arrays, the arrays will be simple-arrays 26 | ;; and thus smaller, and we can share one index for both arrays 27 | (loop 28 | with new-vertices = (make-array (1+ limit) :fill-pointer 0 :element-type 'vec) 29 | and new-axes = (make-array (1+ limit) :fill-pointer 0 :element-type 'poly-axis) 30 | for v in vertices for b = (vec+ offset v) 31 | and a = (vec+ offset (car (last vertices))) then b 32 | for normal = (vec-normalize (vec-perp (vec- b a))) 33 | do (vector-push a new-vertices) 34 | (vector-push (make-poly-axis normal (vec. normal a)) new-axes) 35 | finally (return (values (make-array (1+ limit) :element-type 'vec 36 | :initial-contents new-vertices) 37 | (make-array (1+ limit) :element-type 'poly-axis 38 | :initial-contents new-axes))))) 39 | 40 | (defun validate-vertices (vertices) 41 | "Check that a set of vertices has a correct winding, and that they form a convex polygon." 42 | (loop with tail = (last vertices 2) 43 | for c in vertices 44 | and a = (pop tail) then b 45 | and b = (pop tail) then c 46 | always (minusp (vec-cross (vec- b a) (vec- c b))))) 47 | 48 | (defun make-poly (vertices &key (restitution 0d0) (friction 0d0) (offset +zero-vector+)) 49 | (assert (validate-vertices vertices)) 50 | (aprog1 (%make-poly (length vertices) (float restitution 1d0) (float friction 1d0)) 51 | (setf (values (poly-vertices it) (poly-axes it)) 52 | (compute-new-vertices vertices offset)))) 53 | 54 | (defun num-vertices (poly) 55 | (length (poly-vertices poly))) 56 | 57 | (defun nth-vertex (index poly) 58 | (aref (poly-vertices poly) index)) 59 | 60 | (defun poly-value-on-axis (poly normal distance) 61 | "Returns the minimum distance of the polygon to the axis." 62 | ;; Abandon all hope, ye who enter here! 63 | (declare (optimize speed) (double-float distance) (vec normal)) 64 | (prog* ((transformed-vertices (poly-transformed-vertices poly)) 65 | (min (vec. normal (aref transformed-vertices 0))) 66 | (limit (length transformed-vertices)) 67 | (index 1) 68 | (dot (vec. normal (aref transformed-vertices index)))) 69 | (declare (fixnum index limit) (double-float min dot)) 70 | loop (when (< dot min) (setf min dot)) 71 | (incf index) 72 | (when (= index limit) (go exit)) 73 | (setf dot (vec. normal (aref transformed-vertices index))) 74 | (go loop) 75 | exit (return (- min distance)))) 76 | 77 | (defun poly-contains-vertex-p (poly vertex) 78 | "Returns true if the polygon contains the vertex." 79 | (loop for axis across (poly-transformed-axes poly) 80 | never (> (vec. (poly-axis-normal axis) vertex) 81 | (poly-axis-distance axis)))) 82 | 83 | (declaim (ftype (function (poly vec vec) boolean) partial-poly-contains-vertex-p) 84 | (inline partial-poly-contains-vertex-p)) 85 | (defun partial-poly-contains-vertex-p (poly vertex normal) 86 | "Same as POLY-CONTAINS-VERTEX-P, but ignores faces pointing away from NORMAL." 87 | (loop for axis across (poly-transformed-axes poly) 88 | never (unless (minusp (vec. (poly-axis-normal axis) normal)) 89 | (> (vec. (poly-axis-normal axis) vertex) 90 | (poly-axis-distance axis))))) 91 | 92 | (defmethod compute-shape-bbox ((poly poly)) 93 | (loop for vert across (poly-transformed-vertices poly) 94 | minimize (vec-x vert) into minx 95 | maximize (vec-x vert) into maxx 96 | minimize (vec-y vert) into miny 97 | maximize (vec-y vert) into maxy 98 | finally (return (make-bbox minx miny maxx maxy)))) 99 | 100 | (defun poly-transform-vertices (poly position rotation) 101 | (declare (vec position rotation) (optimize speed)) 102 | (do-vector ((i vertex) (poly-vertices poly)) 103 | (setf (aref (poly-transformed-vertices poly) i) 104 | (vec+ position (vec-rotate vertex rotation))))) 105 | 106 | (defun poly-transform-axes (poly position rotation) 107 | (declare (vec position rotation) (optimize speed)) 108 | (do-vector ((i axis) (poly-axes poly)) 109 | (let ((normal (vec-rotate (poly-axis-normal axis) rotation))) 110 | (setf (aref (poly-transformed-axes poly) i) 111 | (make-poly-axis normal (+ (vec. position normal) (poly-axis-distance axis))))))) 112 | 113 | (defmethod shape-cache-data ((poly poly)) 114 | (with-place (body. body-) (position rotation) (poly-body poly) 115 | (poly-transform-vertices poly body.position body.rotation) 116 | (poly-transform-axes poly body.position body.rotation))) 117 | 118 | (defmethod shape-point-query ((poly poly) point) 119 | (and (bbox-containts-vec-p (poly-bbox poly) point) 120 | (poly-contains-vertex-p poly point))) 121 | 122 | (defmethod shape-segment-query ((poly poly) a b &aux (vertices (poly-transformed-vertices poly)) 123 | ret-poly ret-ratio ret-normal) 124 | (loop 125 | for vert across vertices 126 | for axis across (poly-transformed-axes poly) 127 | for i from 0 128 | do (let* ((normal (poly-axis-normal axis)) 129 | (a-normal (vec. a normal))) 130 | (unless (> (poly-axis-distance axis) a-normal) 131 | (let* ((b-normal (vec. b normal)) 132 | (ratio (/ (- (poly-axis-distance axis) a-normal) 133 | (- b-normal a-normal)))) 134 | (when (<= 0 ratio 1) 135 | (let* ((point (vec-lerp a b ratio)) 136 | (dt (- (vec-cross normal point))) 137 | (dt-min (- (vec-cross normal vert))) 138 | (dt-max (- (vec-cross normal (aref vertices (rem (1+ i) (length vertices))))))) 139 | (when (<= dt-min dt dt-max) 140 | (setf ret-poly poly ret-ratio ratio ret-normal normal)))))))) 141 | (when ret-poly 142 | (values ret-poly ret-ratio ret-normal))) 143 | -------------------------------------------------------------------------------- /src/vec.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (declaim (optimize speed)) 5 | 6 | 7 | ;;; The vector type 8 | 9 | (eval-when (:compile-toplevel :load-toplevel :execute) 10 | (deftype vec () 11 | '(complex double-float)) 12 | 13 | (declaim (ftype (function (real real) vec) vec) 14 | (inline vec)) 15 | (defun vec (x y) 16 | (complex (float x 1d0) (float y 1d0))) 17 | 18 | (declaim (ftype (function (vec) double-float) vec-x vec-y) 19 | (inline vec-x vec-y) ) 20 | (locally (declare (optimize (speed 1))) ; For SBCL float returns 21 | (defun vec-x (vec) 22 | (realpart vec)) 23 | (defun vec-y (vec) 24 | (imagpart vec)))) 25 | 26 | ;;; Helper macros 27 | 28 | (defmacro with-vec (form &body body) 29 | "FORM is either a symbol bound to a `vec', or a list of the form: 30 | (name form) 31 | where NAME is a symbol, and FORM evaluates to a `vec'. 32 | WITH-VEC binds NAME.x and NAME.y in the same manner as `with-accessors'." 33 | (let* ((name (ensure-car form)) 34 | (place (ensure-cadr form)) 35 | (*package* (symbol-package name))) 36 | `(with-place (,(symbolicate name ".") vec-) 37 | (x y) ,place 38 | ,@body))) 39 | 40 | (defmacro with-vecs ((form &rest forms) &body body) 41 | "Convenience macro for nesting WITH-VEC forms" 42 | `(with-vec ,form ,@(if forms `((with-vecs ,forms ,@body)) body))) 43 | 44 | ;;; The zero vector 45 | 46 | (define-constant +zero-vector+ (vec 0 0)) 47 | 48 | (declaim (ftype (function (vec) boolean) vec-zerop) 49 | (inline vec-zerop)) 50 | (defun vec-zerop (vec) 51 | "Checks whether VEC is a zero vector" 52 | (= vec +zero-vector+)) 53 | 54 | ;;; Polar conversions 55 | 56 | (eval-when (:compile-toplevel :load-toplevel :execute) 57 | (deftype radian-angle () 'double-float)) 58 | 59 | (declaim (ftype (function (radian-angle) vec) angle->vec) 60 | (inline angle->vec)) 61 | (defun angle->vec (angle) 62 | "Convert an angle, in radians, to a normalized vector" 63 | (cis angle)) 64 | 65 | (declaim (ftype (function (vec) radian-angle) vec->angle) 66 | (inline vec->angle)) 67 | (defun vec->angle (vec) 68 | "Convert a vector to an angle, in radians." 69 | (declare (vec vec)) 70 | (with-vec vec 71 | (atan vec.y vec.x))) 72 | 73 | ;;; Vector arithmetic 74 | 75 | (declaim (ftype (function (vec vec) boolean) vec-equal) 76 | (inline vec-equal)) 77 | (defun vec-equal (a b) 78 | (= a b)) 79 | 80 | (define-compiler-macro vec+ (&rest rest) 81 | (declare (list rest)) 82 | (cond 83 | ((null rest) 84 | +zero-vector+) 85 | ((= 1 (length rest)) 86 | (car rest)) 87 | (t `(+ ,@rest)))) 88 | 89 | (defun vec+ (&rest vectors) 90 | (apply #'+ vectors)) 91 | 92 | (define-compiler-macro vec- (&rest rest) 93 | (declare (list rest)) 94 | `(- ,@rest)) 95 | 96 | (defun vec- (minuend &rest subtrahends) 97 | (declare (vec minuend)) 98 | (apply #'- minuend subtrahends)) 99 | 100 | (declaim (ftype (function (vec double-float) vec) vec*) 101 | (inline vec*)) 102 | (defun vec* (vec scalar) 103 | "Multiplies VEC by SCALAR" 104 | (declare (vec vec)) 105 | (* vec scalar)) 106 | 107 | (declaim (ftype (function (vec vec) double-float) vec. vec-cross) 108 | (inline vec. vec-cross)) 109 | (defun vec. (v1 v2) 110 | "Dot product of two vectors" 111 | (declare (vec v1 v2)) 112 | (with-vecs (v1 v2) 113 | (+ (* v1.x v2.x) 114 | (* v1.y v2.y)))) 115 | 116 | (defun vec-cross (v1 v2) 117 | "Cross product of two vectors" 118 | (declare (vec v1 v2)) 119 | (with-vecs (v1 v2) 120 | (- (* v1.x v2.y) 121 | (* v1.y v2.x)))) 122 | 123 | ;;; Vector rotations 124 | 125 | (declaim (ftype (function (vec) vec) vec-perp vec-rperp) 126 | (inline vec-perp vec-rperp)) 127 | (defun vec-perp (vec) 128 | "Returns a new vector rotated PI/2 counterclockwise from VEC" 129 | (declare (vec vec)) 130 | (with-vec vec 131 | (vec (- vec.y) vec.x))) 132 | 133 | (defun vec-rperp (vec) 134 | "Returns a new vector rotated PI/2 clockwise from VEC" 135 | (declare (vec vec)) 136 | (with-vec vec 137 | (vec vec.y (- vec.x)))) 138 | 139 | (define-compiler-macro vec-rotate (&whole whole vec-form rot-form) 140 | (if (and (listp rot-form) (eq (car rot-form) 'vec)) 141 | (with-gensyms (vec vec.x vec.y rot.x rot.y) 142 | `(let* ((,vec ,vec-form) 143 | (,vec.x (vec-x ,vec)) 144 | (,vec.y (vec-y ,vec)) 145 | (,rot.x ,(second rot-form)) 146 | (,rot.y ,(third rot-form))) 147 | (vec (- (* ,vec.x ,rot.x) 148 | (* ,vec.y ,rot.y)) 149 | (+ (* ,vec.y ,rot.x) 150 | (* ,vec.x ,rot.y))))) 151 | whole)) 152 | 153 | (declaim (ftype (function (vec vec) vec) vec-rotate vec-unrotate) 154 | (inline vec-rotate vec-unrotate)) 155 | (defun vec-rotate (vec rot) 156 | "Rotates VEC by (vec->angle ROT) radians. ROT should be a unit vec. 157 | This function is symmetric between VEC and ROT." 158 | (declare (vec vec rot)) 159 | (with-vecs (vec rot) 160 | (vec (- (* vec.x rot.x) 161 | (* vec.y rot.y)) 162 | (+ (* vec.x rot.y) 163 | (* vec.y rot.x))))) 164 | 165 | (defun vec-unrotate (vec rot) 166 | "Rotates VEC by (- (vec->angle ROT)) radians. ROT should be a unit vec. 167 | This function is symmetric between VEC and ROT." 168 | (declare (vec vec rot)) 169 | (with-vecs (vec rot) 170 | (vec (+ (* vec.x rot.x) 171 | (* vec.y rot.y)) 172 | (- (* vec.y rot.x) 173 | (* vec.x rot.y))))) 174 | 175 | ;;; More messy stuff 176 | 177 | (defun vec-project (v1 v2) 178 | "Returns the projection of V1 onto V2" 179 | (declare (vec v1 v2)) 180 | (vec* v2 (/ (vec. v1 v2) (vec. v2 v2)))) 181 | 182 | (declaim (ftype (function (vec) (double-float 0d0)) vec-length vec-length-sq) 183 | (inline vec-length vec-length-sq)) 184 | (defun vec-length-sq (vec) 185 | "Returns the square of a vector's length" 186 | (vec. vec vec)) 187 | 188 | (defun vec-length (vec) 189 | "Returns the vector's length" 190 | (sqrt (vec-length-sq vec))) 191 | 192 | (defun vec-lerp (v1 v2 ratio) 193 | "Linear interpolation of the vectors and ratio" 194 | (declare (vec v1 v2)) 195 | (let ((ratio (float ratio 1d0))) 196 | (vec+ (vec* v1 (- 1d0 ratio)) 197 | (vec* v2 ratio)))) 198 | 199 | (defun vec-normalize (vec) 200 | "Normalizes a nonzero vector" 201 | (declare (vec vec)) 202 | (vec* vec (/ (vec-length vec)))) 203 | 204 | (defun vec-normalize-safe (vec) 205 | "Normalizes a vector" 206 | (declare (vec vec)) 207 | (if (vec-zerop vec) +zero-vector+ 208 | (vec-normalize vec))) 209 | 210 | (defun vec-clamp (vec len) 211 | (declare (vec vec)) 212 | (let ((len (float len 1d0))) 213 | (if (and (< len (sqrt most-positive-double-float)) 214 | (> (vec-length-sq vec) (* len len))) 215 | (vec* (vec-normalize vec) len) 216 | vec))) 217 | 218 | (defun vec-dist-sq (v1 v2) 219 | (declare (vec v1 v2)) 220 | (vec-length-sq (vec- v1 v2))) 221 | 222 | (defun vec-dist (v1 v2) 223 | (declare (vec v1 v2)) 224 | (vec-length (vec- v1 v2))) 225 | 226 | (defun vec-near (v1 v2 dist) 227 | (declare (vec v1 v2)) 228 | (let ((dist (float dist 1d0))) 229 | (< (vec-dist-sq v1 v2) 230 | (* dist dist)))) 231 | -------------------------------------------------------------------------------- /demo/demo.lisp: -------------------------------------------------------------------------------- 1 | (defpackage #:squirl-demo-2 2 | (:use :cl :squirl)) 3 | (in-package :squirl-demo-2) 4 | 5 | (defparameter *physics-timestep* 1/100) 6 | (defparameter *accumulator* 0) 7 | 8 | (defparameter *world-x-offset* 0) 9 | (defparameter *world-y-offset* 0) 10 | (defparameter *body-radius* 1) ;;for visualisation 11 | 12 | (defun world-box (a1 b1 a2 b2 a3 b3 a4 b4 space static-body) 13 | (let ((shape (make-segment static-body a1 b1 1.0))) 14 | (setf (shape-restitution shape) 1.0) 15 | (setf (shape-friction shape) 1.0) 16 | 17 | (world-add-static-shape space shape) 18 | 19 | (setf shape (make-segment static-body a2 b2 1.0)) 20 | (setf (shape-restitution shape) 1.0) 21 | (setf (shape-friction shape) 1.0) 22 | (world-add-static-shape space shape) 23 | 24 | (setf shape ( make-segment static-body a3 b3 1.0)) 25 | (setf (shape-restitution shape) 1.0) 26 | (setf (shape-friction shape) 1.0) 27 | (world-add-static-shape space shape) 28 | 29 | (setf shape (make-segment static-body a4 b4 1.0)) 30 | (setf (shape-restitution shape) 1.0) 31 | (setf (shape-friction shape) 1.0) 32 | (world-add-static-shape space shape))) 33 | 34 | (defun init-world () 35 | (reset-shape-id-counter) 36 | (let* ((static-body (make-body most-positive-short-float most-positive-short-float 0 0)) 37 | (world (make-world :iterations 10)) 38 | (body (make-body 100.0 10000.0 0 1 1)) 39 | (shape (make-segment body (vec -75 0) (vec 75 0) 5))) 40 | (world-box (vec -320 -240) (vec -320 240) (vec 320 -240) (vec 320 240) 41 | (vec -320 -240) (vec 320 -240) 42 | (vec -320 240) (vec 320 240) 43 | world static-body) 44 | (world-add-body world body) 45 | (setf (shape-restitution shape) 1.0) 46 | (setf (shape-friction shape) 1.0) 47 | (world-add-shape world shape) 48 | (world-add-constraint world (make-pivot-joint body static-body (vec 0 0) (vec 0 0))) 49 | (return-from init-world world))) 50 | 51 | (defun update (ticks world) 52 | (incf *accumulator* (min ticks 0.5)) 53 | (loop while (>= *accumulator* *physics-timestep*) 54 | do (world-step world *physics-timestep*) 55 | (decf *accumulator* *physics-timestep*))) 56 | 57 | (defun add-circle (world) 58 | (let* ((size 10.0) 59 | (mass 1.0) 60 | (radius (vec-length (vec size size)))) 61 | (let ((body (make-body mass (moment-for-circle mass 1.0 size) 0 0))) 62 | (setf (body-position body) (vec (- (* (/ 1.0 (+ (random 10) 1)) (- 640 (* 2 radius))) (- 320 radius)) (- (* (/ 1.0 (+ (random 10)1)) (- 400 (* 2 radius))) (- 240 radius)))) 63 | (setf (body-velocity body) (vec* (vec (- (* 2 (/ 1.0 (+ 1 (random 10)))) 1) (- (* 2 (/ 1.0 (+ (random 10) 1))) 1)) 200)) 64 | (world-add-body world body) 65 | (let ((shape (make-circle body size))) 66 | (setf (shape-restitution shape) 1.0) 67 | (setf (shape-friction shape) 1.0) 68 | (world-add-shape world shape))))) 69 | 70 | (defun add-box (world) 71 | (let* ((size 10.0) 72 | (mass 1.0) 73 | (verts (make-array 4)) 74 | (radius (vec-length (vec size size)))) 75 | (setf (elt verts 0) (vec (- size) (- size))) 76 | (setf (elt verts 1) (vec (- size) size)) 77 | (setf (elt verts 2) (vec size size)) 78 | (setf (elt verts 3) (vec size (- size))) 79 | (let ((body (make-body mass (moment-for-poly mass 4 verts) 0 0))) 80 | (setf (body-position body) (vec (- (* (/ 1.0 (+ (random 10) 1)) (- 640 (* 2 radius))) (- 320 radius)) (- (* (/ 1.0 (+ (random 10)1)) (- 400 (* 2 radius))) (- 240 radius)))) 81 | (setf (body-velocity body) (vec* (vec (- (* 2 (/ 1.0 (+ 1 (random 10)))) 1) (- (* 2 (/ 1.0 (+ (random 10) 1))) 1)) 200)) 82 | (world-add-body world body) 83 | (let ((shape (make-poly body verts))) 84 | (setf (shape-restitution shape) 1.0) 85 | (setf (shape-friction shape) 1.0) 86 | (world-add-shape world shape))))) 87 | 88 | (defgeneric draw-shape (shape color)) 89 | 90 | (defun body-with-color (color) 91 | (lambda (element) 92 | (draw-body element color))) 93 | 94 | (defun draw-body (body color) 95 | (let ((x (round (vec-x (body-position body)))) 96 | (y (round (vec-y (body-position body))))) 97 | (sdl:draw-filled-circle-* (+ x *world-x-offset*) (+ y *world-y-offset*) *body-radius* :color color))) 98 | 99 | (defun shape-with-color (color) 100 | (lambda (element) 101 | (draw-shape element color))) 102 | 103 | (defmethod draw-shape ((shape circle) color) 104 | (let ((x (round (vec-x (circle-transformed-center shape)))) 105 | (y (round (vec-y (circle-transformed-center shape))))) 106 | (sdl:draw-circle-* (+ x *world-x-offset*) (+ y *world-y-offset*) (round (circle-radius shape)) :color color))) 107 | 108 | (defmethod draw-shape ((shape poly) color) 109 | (let ((1st-vert-x (round (vec-x (elt (poly-transformed-vertices shape) 0)))) 110 | (1st-vert-y (round (vec-y (elt (poly-transformed-vertices shape) 0))))) 111 | (do ((vert1-x 0) (vert1-y 0) (vert2-x 0) (vert2-y 0) 112 | (index 1 (1+ index))) 113 | ((= index (length (poly-transformed-vertices shape))) 114 | (sdl:draw-line-* (+ vert2-x *world-x-offset*) (+ vert2-y *world-y-offset*) (+ 1st-vert-x *world-x-offset*) (+ 1st-vert-y *world-y-offset*) :color color)) 115 | (setf vert1-x (round (vec-x (elt (poly-transformed-vertices shape) (1- index))))) 116 | (setf vert1-y (round (vec-y (elt (poly-transformed-vertices shape) (1- index))))) 117 | (setf vert2-x (round (vec-x (elt (poly-transformed-vertices shape) index)))) 118 | (setf vert2-y (round (vec-y (elt (poly-transformed-vertices shape) index)))) 119 | (sdl:draw-line-* (+ vert1-x *world-x-offset*) (+ vert1-y *world-y-offset*) 120 | (+ vert2-x *world-x-offset*) (+ vert2-y *world-y-offset*) 121 | :color color)))) 122 | 123 | (defmethod draw-shape ((seg segment) color) 124 | (let ((x1 (round (vec-x (segment-trans-a seg)))) 125 | (y1 (round (vec-y (segment-trans-a seg)))) 126 | (x2 (round (vec-x (segment-trans-b seg)))) 127 | (y2 (round (vec-y (segment-trans-b seg))))) 128 | (sdl:draw-line-* (+ x1 *world-x-offset*) (+ y1 *world-y-offset*) (+ x2 *world-x-offset*) (+ y2 *world-y-offset*) :color color))) 129 | 130 | (defun render (world) 131 | (sdl:clear-display sdl:*black*) 132 | (map-world-hash (shape-with-color sdl:*green*) (world-active-shapes world)) 133 | (map-world-hash (shape-with-color sdl:*red*) (world-static-shapes world)) 134 | (map 'vector (body-with-color sdl:*blue*) (world-bodies world)) 135 | (sdl:update-display)) 136 | 137 | (defun now () 138 | (/ (sdl:sdl-get-ticks) 1000)) 139 | 140 | (defun quick-and-dirty (&aux (world (init-world))) 141 | (sdl:with-init () 142 | (sdl:window 800 600 :title-caption "SqirL SDL Demo" :icon-caption "SquirL") 143 | (setf *world-x-offset* (/ 800 2)) 144 | (setf *world-y-offset* (/ 600 2)) 145 | (let ((previous-tick (now))) 146 | (add-box world) 147 | (sdl:with-events () 148 | (:idle () 149 | (let ((now (now))) 150 | (update (- now previous-tick) world) 151 | (setf previous-tick now)) 152 | (render world)) 153 | (:quit-event () t) 154 | (:video-expose-event () 155 | (sdl:update-display)) 156 | (:key-down-event () 157 | (when (sdl:key-down-p :sdl-key-escape) 158 | (sdl:push-quit-event)) 159 | (when (sdl:key-down-p :sdl-key-b) 160 | (add-box world)) 161 | (when (sdl:key-down-p :sdl-key-c) 162 | (add-circle world))))))) 163 | -------------------------------------------------------------------------------- /src/world-hash.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | ;; Eventually, this should be (cons t fixnum) 5 | (deftype handle () 6 | "Used internally to track objects added to the hash" 7 | '(cons t integer)) 8 | (defun make-handle (object) 9 | (cons object 0)) 10 | 11 | (defun handle-object (handle) (car handle)) 12 | (defun handle-stamp (handle) (cdr handle)) 13 | 14 | ;;; Used for the `world-hash-handle-set' 15 | (defun handle-equal (object handle) 16 | (eql object (handle-object handle))) 17 | (defun handle-transform (object) 18 | (make-handle object)) 19 | 20 | (defun make-world-hash-table (size) 21 | (make-array (next-prime size) :initial-element nil)) 22 | 23 | (defstruct (world-hash 24 | (:constructor make-world-hash 25 | (cell-size size &aux (table (make-world-hash-table size))))) 26 | "The spatial hash is SquirL's default (and currently only) spatial index" 27 | (cell-size (assert nil) :type double-float) ; Size of the hash's cells 28 | (handle-set (make-hash-set 0 #'handle-equal) :type hash-set) ; `hash-set' of all handles 29 | (table (assert nil) :type simple-vector) ; Bins in use 30 | ;; (junk nil) ; The "recycle bin" 31 | (stamp 1 :type fixnum) ; Incremented on each query; see `handle-stamp' 32 | ) 33 | 34 | (define-print-object (world-hash)) 35 | 36 | (defun stamp-handle (handle hash) 37 | (setf (cdr handle) (world-hash-stamp hash))) 38 | 39 | (defun world-hash-size (hash) 40 | (length (world-hash-table hash))) 41 | 42 | (defun world-hash-chain (hash index) 43 | (aref (world-hash-table hash) index)) 44 | (defun (setf world-hash-chain) (new-chain hash index) 45 | (setf (aref (world-hash-table hash) index) new-chain)) 46 | 47 | (defun clear-hash-cell (hash index) 48 | "Releases the handles under INDEX in `world-hash' HASH, and links the 49 | list structure into the `world-hash-junk'." 50 | (setf (world-hash-chain hash index) nil) 51 | #+ (or) (do* ((chain (world-hash-chain hash index) next) 52 | ;; We need to hang onto the CDR because we 'recycle' NODE 53 | (next (cdr chain) (cdr chain))) 54 | ((null chain) (setf (world-hash-chain hash index) nil)) 55 | (push-cons chain (world-hash-junk hash)))) 56 | 57 | (defun clear-world-hash (hash) 58 | "Clear all cells in the `world-hash' HASH" 59 | (declare (optimize speed (safety 0))) 60 | (let ((table (world-hash-table hash))) 61 | (declare (simple-vector table)) 62 | (dotimes (index (the fixnum (length table))) 63 | (setf (svref table index) nil)))) 64 | 65 | (defun resize-world-hash (hash new-cell-size new-size) 66 | "Resize `world-hash' HASH to the specified dimensions" 67 | (clear-world-hash hash) 68 | (setf (world-hash-cell-size hash) new-cell-size 69 | (world-hash-table hash) (make-world-hash-table new-size))) 70 | 71 | ;; (defun get-new-node (hash) 72 | ;; "Get a recycled node or cons a new one" 73 | ;; (let ((node (pop (world-hash-junk hash)))) 74 | ;; (if (null node) (cons nil nil) node))) 75 | 76 | ;; (defmacro push-handle (handle hash chain) 77 | ;; (with-gensyms (node) 78 | ;; `(let ((,node (get-new-node ,hash))) 79 | ;; (setf (car ,node) ,handle) 80 | ;; (push-cons ,node ,chain)))) 81 | 82 | (declaim (ftype (function (fixnum fixnum fixnum) fixnum) hash)) 83 | (defun hash (x y n) 84 | "Hash X, Y, and N to generate a hash code" 85 | (mod (the fixnum 86 | (logand (logxor (* x 2185031351) (* y 4232417593)) 87 | most-positive-fixnum)) 88 | n)) 89 | 90 | (defmacro do-bbox ((chain-macro hash-form bbox-form) &body body) 91 | (with-gensyms (hash bbox size dim bb.l bb.r bb.b bb.t i j index) 92 | `(let* ((,hash ,hash-form) 93 | (,bbox ,bbox-form) 94 | (,size (world-hash-size ,hash)) 95 | (,dim (world-hash-cell-size ,hash)) 96 | (,bb.t (floor (/ (bbox-top ,bbox) ,dim))) 97 | (,bb.l (floor (/ (bbox-left ,bbox) ,dim))) 98 | (,bb.r (floor (/ (bbox-right ,bbox) ,dim))) 99 | (,bb.b (floor (/ (bbox-bottom ,bbox) ,dim)))) 100 | (symbol-macrolet ((,chain-macro (world-hash-chain ,hash ,index))) 101 | (loop for ,i from ,bb.l to ,bb.r 102 | do (loop for ,j from ,bb.b to ,bb.t 103 | for ,index = (hash ,i ,j ,size) ,@body)))))) 104 | 105 | (defun hash-handle (hash handle bbox) 106 | (do-bbox (chain hash bbox) 107 | unless (find handle chain) do 108 | (push handle chain))) 109 | 110 | (defun world-hash-insert (hash object id bbox) 111 | (with-accessors ((handle-set world-hash-handle-set)) hash 112 | (let ((handle (make-handle object))) 113 | (hash-set-insert handle-set id handle) 114 | (hash-handle hash handle bbox) 115 | object))) 116 | 117 | (defun world-hash-rehash-object (hash object id) 118 | (with-accessors ((handle-set world-hash-handle-set)) hash 119 | (hash-handle hash (hash-set-find handle-set id object) 120 | (shape-bbox object)))) 121 | 122 | (defun rehash-world-hash (hash) 123 | (clear-world-hash hash) 124 | (map-hash-set (fun (hash-handle hash _ (shape-bbox (handle-object _)))) 125 | (world-hash-handle-set hash))) 126 | 127 | (defun world-hash-remove (hash object id) 128 | (multiple-value-bind (handle foundp) 129 | (hash-set-remove (world-hash-handle-set hash) id object) 130 | (when foundp (handle-object handle)))) 131 | 132 | (defun map-world-hash (function hash) 133 | (map-hash-set (fun (funcall function (handle-object _))) 134 | (world-hash-handle-set hash))) 135 | 136 | (defun map-world-hash-chain (function hash chain object) 137 | (loop for handle in chain 138 | unless (or (= (handle-stamp handle) (world-hash-stamp hash)) 139 | (eq object (handle-object handle)) 140 | (null (handle-object handle))) do 141 | (funcall function object (handle-object handle)) 142 | (stamp-handle handle hash))) 143 | 144 | (defun world-hash-point-query (function hash point) 145 | (prog1 (let* ((dim (world-hash-cell-size hash)) 146 | (idx (with-vec (pt point) 147 | (hash (floor (/ pt.x dim)) (floor (/ pt.y dim)) 148 | (world-hash-size hash))))) 149 | (map-world-hash-chain function hash (world-hash-chain hash idx) point)) 150 | (incf (world-hash-stamp hash)))) 151 | 152 | (defun world-hash-query (function hash object bbox) 153 | (do-bbox (chain hash bbox) 154 | do (map-world-hash-chain function hash chain object)) 155 | (incf (world-hash-stamp hash))) 156 | 157 | (defun world-hash-query-rehash (function hash) 158 | (clear-world-hash hash) 159 | (map-hash-set (fun 160 | (let* ((object (handle-object _))) 161 | (do-bbox (chain-form hash (shape-bbox object)) 162 | for chain = chain-form 163 | unless (find _ chain) do 164 | (map-world-hash-chain function hash chain object) 165 | (push _ chain-form))) 166 | (incf (world-hash-stamp hash))) 167 | (world-hash-handle-set hash))) 168 | 169 | (defun query-segment (function hash chain) 170 | (dolist (handle chain 1.0) 171 | (unless (or (= (handle-stamp handle) (world-hash-stamp hash)) 172 | (null (handle-object handle))) 173 | (stamp-handle handle hash) 174 | (return (funcall function (handle-object handle)))))) 175 | 176 | (defun world-hash-query-segment (function hash vec-a vec-b) 177 | (with-accessors ((cell-size world-hash-cell-size)) hash 178 | (with-vecs ((a (vec* vec-a (/ cell-size))) 179 | (b (vec* vec-b (/ cell-size)))) 180 | (let ((dt/dx (/ (abs (- b.x a.x)))) 181 | (dt/dy (/ (abs (- b.y a.y)))) 182 | (cell-x (floor a.x)) 183 | (cell-y (floor a.y)) 184 | (ratio 0) (exit-ratio 1) 185 | x-inc y-inc next-v next-h) 186 | (if (> b.x a.x) 187 | (setf x-inc 1 next-h (* (- (floor (1+ a.x)) a.x) dt/dx)) 188 | (setf x-inc -1 next-h (* (- a.x (floor a.x)) dt/dx))) 189 | (if (> b.y a.y) 190 | (setf y-inc 1 next-v (* (- (floor (1+ a.x)) a.x) dt/dy)) 191 | (setf y-inc -1 next-v (* (- a.y (floor a.y)) dt/dy))) 192 | (let ((cell-size (world-hash-size hash))) 193 | (loop while (< ratio exit-ratio) for index = (hash cell-x cell-y cell-size) 194 | for new-ratio = (query-segment function hash (world-hash-chain hash index)) 195 | do (setf exit-ratio (min exit-ratio new-ratio)) 196 | (if (< next-v next-h) ; Note indentation 197 | (progn 198 | (incf cell-y y-inc) 199 | (setf ratio next-v) 200 | (incf next-v dt/dy)) 201 | (progn 202 | (incf cell-x x-inc) 203 | (setf ratio next-h) 204 | (incf next-h dt/dx))))) 205 | (incf (world-hash-stamp hash)))))) 206 | -------------------------------------------------------------------------------- /src/arbiter.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (define-constant +bias-coefficient+ 0.1d0 5 | "Determines how fast penetrations resolve themselves.") 6 | 7 | (defstruct (arbiter (:constructor make-arbiter (contacts shape-a shape-b stamp))) 8 | ;; Information on the contact points between the objects 9 | (contacts (assert nil) :type list) 10 | ;; The two shapes involved in the collision 11 | (shape-a (assert nil) :type shape) 12 | (shape-b (assert nil) :type shape) 13 | ;; Calculated by arbiter-prestep 14 | (friction 0d0 :type double-float) 15 | (target-velocity +zero-vector+ :type vec) 16 | ;; Timestamp of the arbiter (from world) 17 | (stamp (assert nil) :type fixnum)) 18 | 19 | (defun arbiter-shapes-equal (arbiter1 arbiter2) 20 | (or (and (eq (arbiter-shape-a arbiter1) (arbiter-shape-a arbiter2)) 21 | (eq (arbiter-shape-b arbiter1) (arbiter-shape-b arbiter2))) 22 | (and (eq (arbiter-shape-b arbiter1) (arbiter-shape-b arbiter2)) 23 | (eq (arbiter-shape-a arbiter1) (arbiter-shape-a arbiter2))))) 24 | 25 | (defun arbiter-has-shapes-p (arbiter shape1 shape2) 26 | (or (and (eq shape1 (arbiter-shape-a arbiter)) 27 | (eq shape2 (arbiter-shape-b arbiter))) 28 | (and (eq shape2 (arbiter-shape-a arbiter)) 29 | (eq shape1 (arbiter-shape-b arbiter))))) 30 | 31 | (defun arbiter-inject (arbiter contacts) 32 | "Replaces ARBITER's contacts with the supplied set, saving state for persistent contacts." 33 | (dolist (old-contact (arbiter-contacts arbiter)) 34 | (dolist (new-contact contacts) 35 | (when (= (contact-hash new-contact) 36 | (contact-hash old-contact)) 37 | (setf (contact-accumulated-normal-impulse new-contact) 38 | (contact-accumulated-normal-impulse old-contact) 39 | (contact-accumulated-frictional-impulse new-contact) 40 | (contact-accumulated-frictional-impulse old-contact))))) 41 | (setf (arbiter-contacts arbiter) contacts) 42 | arbiter) 43 | 44 | (declaim (ftype (function (body body vec vec vec) double-float) k-scalar) 45 | (inline k-scalar)) 46 | (defun k-scalar (body1 body2 r1 r2 normal) 47 | (let ((mass-sum (+ (body-inverse-mass body1) 48 | (body-inverse-mass body2))) 49 | (r1-cross-normal (vec-cross r1 normal)) 50 | (r2-cross-normal (vec-cross r2 normal))) 51 | (+ mass-sum 52 | (* r1-cross-normal r1-cross-normal 53 | (body-inverse-inertia body1)) 54 | (* r2-cross-normal r2-cross-normal 55 | (body-inverse-inertia body2))))) 56 | 57 | (declaim (ftype (function (body body vec vec) vec) relative-velocity) 58 | (inline relative-velocity)) 59 | (defun relative-velocity (body1 body2 r1 r2) 60 | (vec- (vec+ (body-velocity body2) 61 | (vec* (vec-perp r2) 62 | (body-angular-velocity body2))) 63 | (vec+ (body-velocity body1) 64 | (vec* (vec-perp r1) 65 | (body-angular-velocity body1))))) 66 | 67 | (declaim (ftype (function (body body vec vec vec) double-float) normal-relative-velocity) 68 | (inline normal-relative-velocity)) 69 | (defun normal-relative-velocity (body1 body2 r1 r2 normal) 70 | (vec. (relative-velocity body1 body2 r1 r2) normal)) 71 | 72 | (defun arbiter-prestep (arbiter dt-inverse) 73 | (declare (optimize speed) (double-float dt-inverse)) 74 | (let* ((shape-a (arbiter-shape-a arbiter)) 75 | (shape-b (arbiter-shape-b arbiter)) 76 | (body-a (shape-body shape-a)) 77 | (body-b (shape-body shape-b))) 78 | (setf (arbiter-friction arbiter) (* (shape-friction shape-a) 79 | (shape-friction shape-b)) 80 | (arbiter-target-velocity arbiter) (vec- (shape-surface-velocity shape-b) 81 | (shape-surface-velocity shape-a))) 82 | (dolist (contact (arbiter-contacts arbiter)) 83 | (setf (contact-r1 contact) 84 | (vec- (contact-point contact) 85 | (body-position body-a)) 86 | (contact-r2 contact) 87 | (vec- (contact-point contact) 88 | (body-position body-b)) 89 | (contact-normal-mass contact) 90 | (/ (k-scalar body-a body-b 91 | (contact-r1 contact) 92 | (contact-r2 contact) 93 | (contact-normal contact))) 94 | (contact-tangent-mass contact) 95 | (/ (k-scalar body-a body-b 96 | (contact-r1 contact) 97 | (contact-r2 contact) 98 | (vec-perp (contact-normal contact)))) 99 | (contact-bias contact) 100 | (* (- +bias-coefficient+) 101 | dt-inverse 102 | (min 0d0 (+ (contact-distance contact) 103 | +collision-slop+))) 104 | (contact-impulse-bias contact) 105 | 0d0 106 | (contact-bounce contact) 107 | (* (shape-restitution shape-a) 108 | (shape-restitution shape-b) 109 | (normal-relative-velocity body-a body-b 110 | (contact-r1 contact) 111 | (contact-r2 contact) 112 | (contact-normal contact))))))) 113 | 114 | (defun arbiter-apply-cached-impulse (arbiter) 115 | (let ((shape-a (arbiter-shape-a arbiter)) 116 | (shape-b (arbiter-shape-b arbiter))) 117 | (setf (arbiter-friction arbiter) (* (shape-friction shape-a) 118 | (shape-friction shape-b)) 119 | (arbiter-target-velocity arbiter) (vec- (shape-surface-velocity shape-b) 120 | (shape-surface-velocity shape-a))) 121 | (let ((body-a (shape-body shape-a)) 122 | (body-b (shape-body shape-b))) 123 | (dolist (contact (arbiter-contacts arbiter)) 124 | (apply-impulses body-a body-b 125 | (contact-r1 contact) 126 | (contact-r2 contact) 127 | (vec-rotate (contact-normal contact) 128 | (vec (contact-accumulated-normal-impulse contact) 129 | (contact-accumulated-frictional-impulse contact)))))))) 130 | 131 | (declaim (ftype (function (arbiter boolean)) arbiter-apply-impulse)) 132 | (defun arbiter-apply-impulse (arbiter elasticp) 133 | (declare (arbiter arbiter) (optimize speed)) 134 | (let* ((e-coefficient (if elasticp 1d0 0d0)) 135 | (body-a (shape-body (arbiter-shape-a arbiter))) 136 | (body-b (shape-body (arbiter-shape-b arbiter))) 137 | (vb-a (body-velocity-bias body-a)) 138 | (vb-b (body-velocity-bias body-b)) 139 | (avb-a (body-angular-velocity-bias body-a)) 140 | (avb-b (body-angular-velocity-bias body-b))) 141 | (dolist (contact (arbiter-contacts arbiter)) 142 | (flet ((relative-bias-velocity (vb r avb) 143 | (vec+ vb (vec* (vec-perp r) avb)))) 144 | (let* ((n (contact-normal contact)) 145 | (r1 (contact-r1 contact)) 146 | (r2 (contact-r2 contact)) 147 | ;; Relative bias velocities 148 | (vb1 (relative-bias-velocity vb-a r1 avb-a)) 149 | (vb2 (relative-bias-velocity vb-b r2 avb-b)) 150 | (vbn (vec. (vec- vb2 vb1) n))) 151 | ;; Calculate and clamp bias impulse 152 | (let ((jbn (* (- (contact-bias contact) vbn) 153 | (contact-normal-mass contact))) 154 | (jbn-old (contact-impulse-bias contact))) 155 | (setf (contact-impulse-bias contact) (max 0d0 (+ jbn-old jbn)) 156 | jbn (- (contact-impulse-bias contact) jbn-old)) 157 | ;; Apply bias impulse 158 | (body-apply-bias-impulse body-a (vec* n (- jbn)) r1) 159 | (body-apply-bias-impulse body-b (vec* n jbn) r2)) 160 | ;; Calculate relative velocity 161 | (let* ((relative-velocity (relative-velocity body-a body-b r1 r2)) 162 | (n-relative-velocity (vec. relative-velocity n))) 163 | (flet ((calculate-normal-impulse (contact e-coef nrv) 164 | (* (- (+ (* (contact-bounce contact) e-coef) nrv)) 165 | (contact-normal-mass contact)))) 166 | (let ((jn (calculate-normal-impulse contact e-coefficient n-relative-velocity)) 167 | (jn-old (contact-accumulated-normal-impulse contact))) 168 | (setf (contact-accumulated-normal-impulse contact) (max 0d0 (+ jn-old jn)) 169 | jn (- (contact-accumulated-normal-impulse contact) jn-old)) 170 | (let* ((relative-tangent-velocity (vec. (vec+ relative-velocity 171 | (arbiter-target-velocity arbiter)) 172 | (vec-perp n))) 173 | ;; Calculate and clamp friction impulse 174 | (jt-max (* (arbiter-friction arbiter) 175 | (contact-accumulated-normal-impulse contact))) 176 | (jt (* (- relative-tangent-velocity) (contact-tangent-mass contact))) 177 | (jt-old (contact-accumulated-frictional-impulse contact))) 178 | (setf (contact-accumulated-frictional-impulse contact) (clamp (+ jt-old jt) 179 | (- jt-max) jt-max) 180 | jt (- (contact-accumulated-frictional-impulse contact) jt-old)) 181 | (apply-impulses body-a body-b r1 r2 182 | (vec-rotate n (vec jn jt)))))))))))) 183 | -------------------------------------------------------------------------------- /src/body.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defmacro defbody (name) 5 | (let ((*package* (symbol-package name))) 6 | `(progn 7 | (defstruct (,name 8 | (:include body) 9 | (:constructor ,(symbolicate "%MAKE-" name) 10 | (%mass %inertia calculate-inertia-p position 11 | velocity force actor 12 | %angle angular-velocity 13 | &aux 14 | (inverse-mass 15 | (without-floating-point-underflow 16 | (if (zerop %mass) 0d0 (/ %mass)))) 17 | (inverse-inertia 18 | (without-floating-point-underflow 19 | (if (zerop %inertia) 0d0 (/ %inertia)))) 20 | (rotation (angle->vec %angle)))))) 21 | (defun ,(symbolicate "MAKE-" name) 22 | (&key (mass 0d0) (inertia most-positive-double-float) (calculate-inertia-p t) 23 | (position +zero-vector+) (velocity +zero-vector+) (force +zero-vector+) 24 | actor shapes (angle 0d0) (angular-velocity 0d0)) 25 | (aprog1 (,(symbolicate "%MAKE-" name) 26 | (float mass 0d0) (float inertia 1d0) calculate-inertia-p position velocity 27 | force actor (float angle 0d0) (float angular-velocity 0d0)) 28 | (attach-shapes shapes it)))))) 29 | 30 | (defstruct (body 31 | (:constructor 32 | %make-body (%mass %inertia calculate-inertia-p 33 | position velocity force actor %angle angular-velocity 34 | &aux 35 | (inverse-mass 36 | (without-floating-point-underflow 37 | (if (zerop %mass) 0d0 (/ %mass)))) 38 | (inverse-inertia 39 | (without-floating-point-underflow 40 | (if (zerop %inertia) 0d0 (/ %inertia)))) 41 | (rotation (angle->vec %angle))))) 42 | world ; world that this body is attached to, if any. 43 | actor ; Actor used for the COLLIDE "callback" 44 | %shapes ; shapes associated with this body. 45 | ;; Mass properties, and cached inverses 46 | (%mass (assert nil) :type double-float) 47 | (inverse-mass (assert nil) :type double-float) 48 | (%inertia (assert nil) :type double-float) 49 | (inverse-inertia (assert nil) :type double-float) 50 | (calculate-inertia-p t :type boolean) 51 | ;; Linear components of motion 52 | (position +zero-vector+ :type vec) 53 | (velocity +zero-vector+ :type vec) 54 | (force +zero-vector+ :type vec) 55 | ;; Angular components of motion, and cached rotation vector 56 | (%angle 0d0 :type double-float) 57 | (rotation +initial-rotation+ :type vec) 58 | (angular-velocity 0d0 :type double-float) 59 | (torque 0d0 :type double-float) 60 | ;; Velocity bias values used when solving penetrations and correcting constraints. 61 | (velocity-bias +zero-vector+ :type vec) 62 | (angular-velocity-bias 0d0 :type double-float)) 63 | 64 | (defun make-body (&key (mass 0d0) (inertia most-positive-double-float) (calculate-inertia-p t) 65 | (position +zero-vector+) (velocity +zero-vector+) (force +zero-vector+) actor 66 | shapes (angle 0d0) (angular-velocity 0d0)) 67 | (let ((body (%make-body (float mass 0d0) (float inertia 1d0) calculate-inertia-p position velocity 68 | force actor (float angle 0d0) (float angular-velocity 0d0)))) 69 | (attach-shapes shapes body) 70 | body)) 71 | 72 | (defun staticp (body) 73 | (when (= 0 (body-mass body)) t)) 74 | 75 | (defun body-attached-p (body world) 76 | (eq world (body-world body))) 77 | 78 | (defun body-shapes (body) 79 | (body-%shapes body)) 80 | 81 | (define-print-object (body) 82 | (format t "~@[Actor: ~a, ~]Mass: ~a, Inertia: ~a" 83 | (body-actor body) (body-mass body) (body-inertia body))) 84 | 85 | ;;; Wraps the mass, inertia, and angle slots so that setting them updates 86 | ;;; the inverse-mass, inverse-inertia, and rotation slots. 87 | (macrolet ((wrap (external internal cached wrapper) 88 | `(progn (defun ,external (body) (,internal body)) 89 | (defun (setf ,external) (new-value body) 90 | (setf (,internal body) new-value 91 | (,cached body) (,wrapper new-value)))))) 92 | (wrap body-mass body-%mass body-inverse-mass maybe-inverse) 93 | (wrap body-inertia body-%inertia body-inverse-inertia maybe-inverse) 94 | (wrap body-angle body-%angle body-rotation angle->vec)) 95 | 96 | (defgeneric body-update-velocity (body gravity damping dt) 97 | (:method ((body body) gravity damping dt) 98 | (declare (optimize speed) (double-float dt) (vec gravity)) 99 | (with-accessors ((angular-velocity body-angular-velocity) 100 | (inv-inertia body-inverse-inertia) 101 | (inv-mass body-inverse-mass) 102 | (velocity body-velocity) 103 | (torque body-torque) 104 | (force body-force)) body 105 | (setf velocity 106 | (vec+ (vec* velocity damping) 107 | (vec* (vec+ gravity (vec* force inv-mass)) dt))) 108 | (setf angular-velocity 109 | (+ (* angular-velocity damping) 110 | (* torque inv-inertia dt))) 111 | (values)))) 112 | 113 | (defgeneric body-update-position (body dt) 114 | (:method ((body body) dt) 115 | (with-accessors ((angular-velocity-bias body-angular-velocity-bias) 116 | (angular-velocity body-angular-velocity) 117 | (velocity-bias body-velocity-bias) 118 | (position body-position) 119 | (velocity body-velocity) 120 | (angle body-angle)) body 121 | (setf position (vec+ position (vec* (vec+ velocity velocity-bias) dt))) 122 | (incf angle (* (+ angular-velocity angular-velocity-bias) dt)) 123 | (setf velocity-bias +zero-vector+) 124 | (setf angular-velocity-bias 0d0)))) 125 | 126 | (defun body-slew (body pos dt) 127 | "Modify the velocity of the body so that it will move to the specified absolute coordinates in 128 | the next timestep. 129 | Intended for objects that are moved manually with a custom velocity integration function." 130 | (setf (body-velocity body) 131 | (vec* (vec- pos (body-position body)) 132 | (/ dt)))) 133 | 134 | (defun body-local->world (body vec) 135 | "Convert body local to world coordinates." 136 | (vec+ (body-position body) 137 | (vec-rotate vec (body-rotation body)))) 138 | 139 | (defun world->body-local (body vec) 140 | "Convert world to body local coordinates" 141 | (vec-unrotate (vec- vec (body-position body)) 142 | (body-rotation body))) 143 | 144 | (defun body-apply-impulse (body impulse relative) 145 | "Apply an impulse (in world coordinates) to the body at a point relative to the center of 146 | gravity (also in world coordinates)." 147 | (declare (optimize speed)) 148 | (with-accessors ((angular-velocity body-angular-velocity) 149 | (inverse-inertia body-inverse-inertia) 150 | (inverse-mass body-inverse-mass) 151 | (velocity body-velocity)) body 152 | (setf velocity (vec+ velocity (vec* impulse inverse-mass))) 153 | (incf angular-velocity (* inverse-inertia (vec-cross relative impulse))) 154 | (values))) 155 | 156 | (defun body-apply-bias-impulse (body impulse relative) 157 | ;; From C: "Not intended for external use. Used by cpArbiter.c and cpConstraint.c." 158 | (declare (optimize speed)) 159 | (with-accessors ((angular-velocity-bias body-angular-velocity-bias) 160 | (inverse-inertia body-inverse-inertia) 161 | (inverse-mass body-inverse-mass) 162 | (velocity-bias body-velocity-bias)) body 163 | (setf velocity-bias (vec+ velocity-bias (vec* impulse inverse-mass))) 164 | (incf angular-velocity-bias (* inverse-inertia (vec-cross relative impulse)))) 165 | (values)) 166 | 167 | (defun body-reset-forces (body) 168 | "Zero the forces on a body." 169 | (setf (body-force body) +zero-vector+ 170 | (body-torque body) 0d0)) 171 | 172 | (defun body-apply-force (body force r) 173 | "Apply a force (in world coordinates) to a body at a point relative to the center 174 | of gravity (also in world coordinates)." 175 | (setf (body-force body) (vec+ (body-force body) force)) 176 | (incf (body-torque body) (vec-cross r force))) 177 | 178 | (defun apply-damped-spring (body1 body2 anchor1 anchor2 rlen k dmp dt) 179 | "Apply a damped spring force between two bodies. 180 | Warning: Large damping values can be unstable. Use a DAMPED-SPRING constraint for this instead." 181 | (setf anchor1 (vec-rotate anchor1 (body-rotation body1)) 182 | anchor2 (vec-rotate anchor2 (body-rotation body2))) 183 | (let* ((delta (vec- (vec+ (body-position body2) anchor2) 184 | (vec+ (body-position body1) anchor1))) 185 | (normal (vec-normalize-safe delta)) 186 | (f-spring (* k (- (vec-length delta) rlen))) 187 | ;; Calculate the world relative velocities of the anchor points. 188 | (v1 (vec+ (body-velocity body1) 189 | (vec* (vec-perp anchor1) (body-angular-velocity body1)))) 190 | (v2 (vec+ (body-velocity body2) 191 | (vec* (vec-perp anchor2) (body-angular-velocity body2)))) 192 | ;; Calculate the damping force. 193 | ;; This really should be in the impulse solve and can produce problems when 194 | ;; using large damping values. 195 | (f-damp (* (vec. (vec- v2 v1) normal) 196 | (min dmp (/ (* dt (+ (body-inverse-mass body1) 197 | (body-inverse-mass body2))))))) 198 | (f (vec* normal (+ f-spring f-damp)))) 199 | ;; Apply! 200 | (body-apply-force body1 f anchor1) 201 | (body-apply-force body2 (vec- f) anchor2))) 202 | -------------------------------------------------------------------------------- /src/shape.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defparameter *shape-id-counter* 0) 5 | 6 | (defun reset-shape-id-counter () 7 | (setf *shape-id-counter* 0)) 8 | 9 | ;;; 10 | ;;; Shape 11 | ;;; 12 | (defstruct shape 13 | body ; Body to which the shape is attached 14 | bbox ; Cached BBox for the shape 15 | ;; Surface Properties 16 | (restitution 0d0 :type double-float) ; Coefficient of restitution. 17 | (friction 0d0 :type double-float) ; Coefficient of friction. 18 | (surface-velocity +zero-vector+ :type vec) ; Surface velocity used when solving for friction 19 | ;; Unique ID, used internally for hashing 20 | (id (prog1 *shape-id-counter* (incf *shape-id-counter*)) 21 | :type (unsigned-byte #. (integer-length most-positive-fixnum)))) 22 | 23 | (defgeneric print-shape (shape) 24 | (:method-combination progn :most-specific-last)) 25 | 26 | (define-print-object (shape) 27 | (print-shape shape)) 28 | 29 | (defmethod print-shape progn ((shape shape)) 30 | (format t "Body: ~a, " 31 | (remove #\Space 32 | (with-output-to-string (*standard-output*) 33 | (print-unreadable-object ((shape-body shape) 34 | *standard-output* :identity t :type nil)))))) 35 | 36 | (defun calculate-inertia (body) 37 | (if (= 0 (body-mass body)) 38 | most-positive-double-float 39 | (float 40 | (loop for shape in (body-shapes body) 41 | summing 42 | (etypecase shape 43 | (circle (moment-of-inertia-for-circle (body-mass body) 1 44 | (circle-radius shape) (circle-center shape))) 45 | (segment (moment-of-inertia-for-segment (body-mass body) 46 | (segment-a shape) (segment-b shape))) 47 | (poly (moment-of-inertia-for-poly (body-mass body) (poly-vertices shape))))) 48 | 1d0))) 49 | 50 | (defun %attach-shape (shape body) 51 | (setf (shape-body shape) body) 52 | (pushnew shape (body-%shapes (shape-body shape))) 53 | (shape-cache-data shape) 54 | (when (body-world body) 55 | (if (staticp body) 56 | (world-add-static-shape (body-world body) shape) 57 | (world-add-active-shape (body-world body) shape)))) 58 | 59 | (defun attach-shape (shape body) 60 | "Attaches SHAPE to BODY. All shapes must be attached to a body before they're used." 61 | (%attach-shape shape body) 62 | (when (body-calculate-inertia-p body) 63 | (setf (body-inertia body) (calculate-inertia body))) 64 | body) 65 | 66 | (defun attach-shapes (shapes body) 67 | "Attaches multiple shapes to BODY. This function adds all the shapes before recalculating inertia." 68 | (map nil (fun (%attach-shape _ body)) shapes) 69 | (when (body-calculate-inertia-p body) 70 | (setf (body-inertia body) (calculate-inertia body))) 71 | body) 72 | 73 | (defun detach-shape (shape body) 74 | "Detaches SHAPE from BODY. If BODY is already attached to a world, the shape is removed from there." 75 | (setf (body-%shapes body) (delete shape (body-%shapes body))) 76 | (setf (shape-body shape) nil) 77 | (when (body-world body) 78 | (world-remove-shape (body-world body) shape)) 79 | (when (body-calculate-inertia-p body) 80 | (setf (body-inertia body) (calculate-inertia body))) 81 | shape) 82 | 83 | (defgeneric compute-shape-bbox (shape) 84 | (:documentation "Compute the BBox of a shape.")) 85 | 86 | (defgeneric shape-cache-data (shape) 87 | (:documentation "Cache any cachable data about SHAPE") 88 | (:method :after ((shape shape)) 89 | (setf (shape-bbox shape) (compute-shape-bbox shape)))) 90 | 91 | (defun point-inside-shape-p (shape point) 92 | (shape-point-query shape point)) 93 | 94 | (defgeneric shape-point-query (shape point) 95 | (:documentation "Test if a point lies within a shape.")) 96 | 97 | (defun segment-intersects-shape-p (shape segment-point-a segment-point-b) 98 | "Tests if the line segment that runs between point A and point B intersects SHAPE." 99 | (shape-segment-query shape segment-point-a segment-point-b)) 100 | 101 | (defgeneric shape-segment-query (shape a b)) 102 | 103 | ;;; 104 | ;;; Circles 105 | ;;; 106 | (defstruct (circle (:constructor %make-circle (radius center restitution friction)) 107 | (:include shape)) 108 | (radius (assert nil) :type double-float) 109 | ;; Center, in body-relative and world coordinates 110 | (center (assert nil) :type vec) 111 | (transformed-center +zero-vector+ :type vec)) 112 | 113 | (defun make-circle (radius &key (center +zero-vector+) (restitution 0d0) (friction 0d0)) 114 | (%make-circle (float radius 1d0) center (float restitution 1d0) (float friction 1d0))) 115 | 116 | (defmethod print-shape progn ((circle circle)) 117 | (format t "Center: ~a, Radius: ~a" 118 | (circle-center circle) (circle-radius circle))) 119 | 120 | (defmethod compute-shape-bbox ((circle circle)) 121 | (declare (optimize speed)) 122 | (with-vec (vec (circle-transformed-center circle)) 123 | (let ((r (circle-radius circle))) 124 | (make-bbox (- vec.x r) (- vec.y r) (+ vec.x r) (+ vec.y r))))) 125 | 126 | (defmethod shape-cache-data ((circle circle)) 127 | (with-place (body. body-) (position rotation) (circle-body circle) 128 | (setf (circle-transformed-center circle) 129 | (vec+ body.position (vec-rotate (circle-center circle) body.rotation))))) 130 | 131 | (defmethod shape-point-query ((circle circle) point) 132 | (vec-near (circle-transformed-center circle) point (circle-radius circle))) 133 | 134 | (defmethod shape-segment-query ((circle circle) a b) 135 | (let ((center (circle-transformed-center circle)) 136 | (radius (circle-radius circle))) 137 | (circle-segment-query circle center radius a b))) 138 | 139 | (defun circle-segment-query (shape center radius a b) 140 | (let* ((a (vec- a center)) 141 | (b (vec- b center)) 142 | (qa (+ (- (vec. a a) (* 2 (vec. a b))) (vec. b b))) 143 | (qb (- (* 2 (vec. a b)) (* 2 (vec. a a)))) 144 | (qc (- (vec. a a) (expt radius 2))) 145 | (det (- (expt qb 2) (* 4 qa qc)))) 146 | (unless (minusp det) 147 | (let ((ratio (/ (- (- qb) (sqrt det)) 148 | (* 2 qa)))) 149 | (when (<= 0 ratio 1) 150 | (values shape ratio 151 | (vec-normalize (vec-lerp a b ratio)))))))) 152 | 153 | ;;; 154 | ;;; Segments 155 | ;;; 156 | (defstruct (segment (:constructor %make-segment 157 | (a b friction restitution radius 158 | &aux (normal (vec-perp (vec-normalize (vec- b a)))))) 159 | (:include shape)) 160 | radius ; Thickness 161 | ;; Body-relative endpoints & normal 162 | a b normal 163 | ;; World-relative endpoints & normal 164 | trans-a trans-b trans-normal) 165 | 166 | (defun make-segment (a b &key (friction 0d0) (restitution 0d0) (radius 1d0)) 167 | (%make-segment a b (float friction 1d0) (float restitution 1d0) (float radius 1d0))) 168 | 169 | (defmethod print-shape progn ((segment segment)) 170 | (format t "Point A: ~a, Point B: ~a, Radius: ~a" 171 | (segment-a segment) (segment-b segment) 172 | (segment-radius segment))) 173 | 174 | (defmethod compute-shape-bbox ((seg segment)) 175 | (with-place (|| segment-) ((ta trans-a) (tb trans-b) (r radius)) seg 176 | (with-vecs (ta tb) 177 | (make-bbox (- (min ta.x tb.x) r) (- (min ta.y tb.y) r) 178 | (+ (max ta.x tb.x) r) (+ (max ta.y tb.y) r))))) 179 | 180 | (defmethod shape-cache-data ((seg segment)) 181 | (with-place (seg.t segment-trans-) (a b normal) seg 182 | (with-place (seg. segment-) (a b normal body) seg 183 | (with-place (body. body-) (position rotation) seg.body 184 | (flet ((rotate (vec) (vec-rotate vec body.rotation))) 185 | (setf seg.ta (vec+ body.position (rotate seg.a)) 186 | seg.tb (vec+ body.position (rotate seg.b)) 187 | seg.tnormal (rotate seg.normal))))))) 188 | 189 | (defmethod shape-point-query ((seg segment) point) 190 | (when (bbox-containts-vec-p (shape-bbox seg) point) 191 | (with-accessors ((seg-ta segment-trans-a) (seg-tb segment-trans-b) (seg-r segment-radius) 192 | (seg-a segment-a) (seg-b segment-b) (seg-tnormal segment-trans-normal) 193 | (seg-normal segment-normal)) 194 | seg 195 | ;; calculate normal distance from segment 196 | (let* ((dn (- (vec. seg-tnormal point) (vec. seg-ta seg-tnormal))) 197 | (dist (- (abs dn) seg-r))) 198 | (if (plusp dist) 199 | (return-from shape-point-query nil) 200 | ;; calculate tangential distance along segment 201 | (let ((dt (- (vec-cross seg-tnormal point))) 202 | (dt-min (- (vec-cross seg-tnormal seg-ta))) 203 | (dt-max (- (vec-cross seg-tnormal seg-tb)))) 204 | ;; decision tree to decide which feature of the segment to collide with 205 | (if (<= dt dt-min) 206 | (if (< dt (- dt-min seg-r)) 207 | (return-from shape-point-query nil) 208 | (return-from shape-point-query (< (vec-length-sq (vec- seg-ta point)) 209 | (expt seg-r 2)))) 210 | (if (< dt dt-max) 211 | (return-from shape-point-query t) 212 | (if (< dt (+ dt-max seg-r)) 213 | (return-from shape-point-query 214 | (< (vec-length-sq (vec- seg-tb point)) 215 | (expt seg-r 2))) 216 | (return-from shape-point-query nil)))))))))) 217 | 218 | (defmethod shape-segment-query ((seg segment) a b) 219 | (let ((n (segment-trans-normal seg))) 220 | (when (< (vec. a n) (vec. (segment-trans-a seg) n)) 221 | (setf n (vec- n))) 222 | (let* ((an (vec. a n)) 223 | (bn (vec. b n)) 224 | (d (+ (vec. (segment-trans-a seg) n) (segment-radius seg))) 225 | (ratio (/ (- d an) (- bn an)))) 226 | (when (< 0 ratio 1) 227 | (let* ((point (vec-lerp a b ratio)) 228 | (dt (- (vec-cross (segment-trans-normal seg) point))) 229 | (dt-min (- (vec-cross (segment-trans-normal seg) (segment-trans-a seg)))) 230 | (dt-max (- (vec-cross (segment-trans-normal seg) (segment-trans-b seg))))) 231 | (when (< dt-min dt dt-max) 232 | (return-from shape-segment-query (values seg ratio n))))) 233 | (unless (zerop (segment-radius seg)) 234 | (multiple-value-bind (shape1 t1 n1) 235 | (circle-segment-query seg (segment-trans-a seg) (segment-radius seg) a b) 236 | (multiple-value-bind (shape2 t2 n2) 237 | (circle-segment-query seg (segment-trans-b seg) (segment-radius seg) a b) 238 | (cond ((and shape1 (null shape2)) 239 | (values shape1 t1 n1)) 240 | ((and shape2 (null shape1)) 241 | (values shape2 t2 n2)) 242 | (t (if (< t1 t2) 243 | (values shape1 t1 n1) 244 | (values shape2 t2 n2)))))))))) 245 | -------------------------------------------------------------------------------- /demo/draw-world.lisp: -------------------------------------------------------------------------------- 1 | (in-package :squirl-demo) 2 | 3 | ;;; 4 | ;;; Configuration 5 | ;;; 6 | (defparameter *line-color* '(0 0 0 1)) 7 | (defparameter *collision-color* '(1 0 0 1)) 8 | (defparameter *body-color* '(0 0 1 1)) 9 | (defparameter *line-width* 2.5) 10 | (defparameter *bb-color* '(1 0 0 1)) 11 | (defparameter *bb-line-width* 2) 12 | 13 | ;;; 14 | ;;; Primitives 15 | ;;; 16 | (defun draw-circle (x y radius &key (resolution 10) (filled t)) 17 | (let* ((theta (* 2 (/ pi resolution))) 18 | (tangential-factor (tan theta)) 19 | (radial-factor (- 1 (cos theta)))) 20 | (gl:with-primitives (if filled :triangle-fan :line-loop) 21 | (loop with curr-x = (+ x radius) 22 | with curr-y = y 23 | repeat resolution 24 | do (gl:vertex curr-x curr-y ) 25 | (let ((tx (- (- curr-y y))) 26 | (ty (- curr-x x))) 27 | (incf curr-x (* tx tangential-factor)) 28 | (incf curr-y (* ty tangential-factor))) 29 | (let ((rx (- x curr-x)) 30 | (ry (- y curr-y))) 31 | (incf curr-x (* rx radial-factor)) 32 | (incf curr-y (* ry radial-factor))))))) 33 | 34 | (defun draw-line (x1 y1 x2 y2) 35 | (gl:with-primitives :lines 36 | (gl:vertex x1 y1) 37 | (gl:vertex x2 y2))) 38 | 39 | (defun draw-poly (vertices &key (filled t)) 40 | (gl:with-primitives (if filled :polygon :lines) 41 | (loop for i below (length vertices) 42 | for a = (elt vertices i) 43 | for b = (elt vertices (mod (1+ i) (length vertices))) 44 | do (gl:vertex (vec-x a) (vec-y a)) 45 | (gl:vertex (vec-x b) (vec-y b))))) 46 | 47 | ;;; 48 | ;;; Bodies and shapes 49 | ;;; 50 | (defparameter *color-hash* (make-hash-table :test 'eq)) 51 | (defun clear-color-hash () 52 | (clrhash *color-hash*)) 53 | 54 | (defun ensure-color (shape) 55 | (or (gethash shape *color-hash*) 56 | (setf (gethash shape *color-hash*) 57 | (list (random 0.9) (random 0.9) (random 0.9) 1)))) 58 | 59 | (defun draw-body (body) 60 | (map nil #'draw-shape (body-shapes body))) 61 | 62 | (defun draw-bbox (body) 63 | (map nil #'draw-shape-bbox (body-shapes body))) 64 | 65 | (defun draw-shape-bbox (shape) 66 | (let ((bbox (squirl::shape-bbox shape))) 67 | (apply #'gl:color *bb-color*) 68 | (gl:line-width *bb-line-width*) 69 | (gl:with-primitives :line-loop 70 | (gl:vertex (squirl::bbox-left bbox) (squirl::bbox-bottom bbox)) 71 | (gl:vertex (squirl::bbox-left bbox) (squirl::bbox-top bbox)) 72 | (gl:vertex (squirl::bbox-right bbox) (squirl::bbox-top bbox)) 73 | (gl:vertex (squirl::bbox-right bbox) (squirl::bbox-bottom bbox))))) 74 | 75 | (defgeneric draw-shape (shape)) 76 | (defmethod draw-shape :before (shape) 77 | (apply #'gl:color (ensure-color shape))) 78 | 79 | (defmethod draw-shape ((circle circle)) 80 | (let* ((body (shape-body circle)) 81 | (center (circle-transformed-center circle)) 82 | (x (vec-x center)) 83 | (y (vec-y center)) 84 | (radius (circle-radius circle)) 85 | (edge (vec* (body-rotation body) radius)) 86 | (edge-t (vec+ edge center))) 87 | (draw-circle x y (round radius) :resolution 30 :filled t) 88 | (gl:color 0 0 0) 89 | (gl:line-width *line-width*) 90 | (draw-circle x y (round radius) :resolution 30 :filled nil) 91 | (draw-line (vec-x edge-t) (vec-y edge-t) (vec-x center) (vec-y center)))) 92 | 93 | 94 | 95 | (defparameter *pill-var* '((0.0000 . 1.0000) 96 | (0.2588 . 0.9659) 97 | (0.5000 . 0.8660) 98 | (0.7071 . 0.7071) 99 | (0.8660 . 0.5000) 100 | (0.9659 . 0.2588) 101 | (1.0000 . 0.0000) 102 | (0.9659 . -0.2588) 103 | (0.8660 . -0.5000) 104 | (0.7071 . -0.7071) 105 | (0.5000 . -0.8660) 106 | (0.2588 . -0.9659) 107 | (0.0000 . -1.0000) 108 | (0.0000 . -1.0000) 109 | (-0.2588 . -0.9659) 110 | (-0.5000 . -0.8660) 111 | (-0.7071 . -0.7071) 112 | (-0.8660 . -0.5000) 113 | (-0.9659 . -0.2588) 114 | (-1.0000 . -0.0000) 115 | (-0.9659 . 0.2588) 116 | (-0.8660 . 0.5000) 117 | (-0.7071 . 0.7071) 118 | (-0.5000 . 0.8660) 119 | (-0.2588 . 0.9659) 120 | (0.0000 . 1.0000))) 121 | 122 | (defmethod draw-shape ((seg segment)) 123 | (let ((a (vec+ (body-position (shape-body seg)) 124 | (vec-rotate (squirl::segment-a seg) 125 | (body-rotation (shape-body seg))))) 126 | (b (vec+ (body-position (shape-body seg)) 127 | (vec-rotate (squirl::segment-b seg) 128 | (body-rotation (shape-body seg)))))) 129 | (if (< 0 (squirl::segment-radius seg)) 130 | (let* ((delta (vec- b a)) 131 | (length (/ (vec-length delta) (squirl::segment-radius seg))) 132 | (verts (loop with verts = (copy-tree *pill-var*) 133 | for i below (/ (length *pill-var*) 2) 134 | do (incf (car (elt verts i)) length) 135 | finally (return verts)))) 136 | (gl:with-pushed-matrix 137 | (let ((x (vec-x a)) 138 | (y (vec-y a)) 139 | (cos (/ (vec-x delta) length)) 140 | (sin (/ (vec-y delta) length))) 141 | (gl:mult-matrix (make-array '(4 4) 142 | :initial-contents 143 | (list (list cos sin 0 0) 144 | (list (- sin) cos 0 0) 145 | (list 0 0 1 1) 146 | (list x y 0 1)))) 147 | (gl:with-primitives :triangle-fan 148 | (loop for (x . y) in verts 149 | do (gl:vertex x y))) 150 | (apply #'gl:color *line-color*) 151 | (gl:with-primitives :line-loop 152 | (loop for (x . y) in verts 153 | do (gl:vertex x y)))))) 154 | (progn (gl:line-width (squirl::segment-radius seg)) 155 | (draw-line (vec-x a) (vec-y a) (vec-x b) (vec-y b)))))) 156 | 157 | (defmethod draw-shape ((poly poly)) 158 | (let ((vertices (poly-transformed-vertices poly))) 159 | (draw-poly vertices :filled t) 160 | (gl:color 0 0 0) 161 | (gl:line-width *line-width*) 162 | (draw-poly vertices :filled nil))) 163 | 164 | ;;; 165 | ;;; Constraints 166 | ;;; 167 | (defgeneric draw-constraint (constraint)) 168 | 169 | (defmethod draw-constraint ((constraint squirl::constraint)) 170 | (let* ((position-a (body-position (constraint-body-a constraint))) 171 | (position-b (body-position (constraint-body-b constraint)))) 172 | (gl:point-size 5) 173 | (gl:with-primitives :points 174 | (gl:vertex (vec-x position-a) (vec-y position-a)) 175 | (gl:vertex (vec-x position-b) (vec-y position-b))))) 176 | 177 | (defmethod draw-constraint ((joint pivot-joint)) 178 | (let* ((body-a (constraint-body-a joint)) 179 | (body-b (constraint-body-b joint)) 180 | (point-a (vec+ (body-position body-a) 181 | (vec-rotate (squirl::pivot-joint-anchor1 joint) 182 | (body-rotation body-a)))) 183 | (point-b (vec+ (body-position body-b) 184 | (vec-rotate (squirl::pivot-joint-anchor2 joint) 185 | (body-rotation body-b))))) 186 | (gl:point-size 10) 187 | (gl:with-primitives :points 188 | (gl:vertex (vec-x point-a) (vec-y point-a)) 189 | (gl:vertex (vec-x point-b) (vec-y point-b))))) 190 | 191 | (defmethod draw-constraint ((joint pin-joint)) 192 | (let* ((body-a (constraint-body-a joint)) 193 | (body-b (constraint-body-b joint)) 194 | (point-a (vec+ (body-position body-a) 195 | (vec-rotate (squirl::pin-joint-anchor1 joint) 196 | (body-rotation body-a)))) 197 | (point-b (vec+ (body-position body-b) 198 | (vec-rotate (squirl::pin-joint-anchor2 joint) 199 | (body-rotation body-b))))) 200 | (gl:point-size 5) 201 | (gl:with-primitives :points 202 | (gl:vertex (vec-x point-a) (vec-y point-a)) 203 | (gl:vertex (vec-x point-b) (vec-y point-b))) 204 | (gl:with-primitives :lines 205 | (gl:vertex (vec-x point-a) (vec-y point-a)) 206 | (gl:vertex (vec-x point-b) (vec-y point-b))))) 207 | 208 | (defmethod draw-constraint ((joint slide-joint)) 209 | (let* ((body-a (constraint-body-a joint)) 210 | (body-b (constraint-body-b joint)) 211 | (point-a (vec+ (body-position body-a) 212 | (vec-rotate (squirl::slide-joint-anchor1 joint) 213 | (body-rotation body-a)))) 214 | (point-b (vec+ (body-position body-b) 215 | (vec-rotate (squirl::slide-joint-anchor2 joint) 216 | (body-rotation body-b))))) 217 | (gl:point-size 5) 218 | (gl:with-primitives :points 219 | (gl:vertex (vec-x point-a) (vec-y point-a)) 220 | (gl:vertex (vec-x point-b) (vec-y point-b))) 221 | (gl:with-primitives :lines 222 | (gl:vertex (vec-x point-a) (vec-y point-a)) 223 | (gl:vertex (vec-x point-b) (vec-y point-b))))) 224 | 225 | (defmethod draw-constraint ((joint breakable-joint)) 226 | (draw-constraint (squirl::breakable-joint-delegate joint))) 227 | 228 | (defmethod draw-constraint ((spring damped-spring)) 229 | (let* ((body-a (constraint-body-a spring)) 230 | (body-b (constraint-body-b spring)) 231 | (point-a (vec+ (body-position body-a) 232 | (vec-rotate (squirl::damped-spring-anchor1 spring) 233 | (body-rotation body-a)))) 234 | (point-b (vec+ (body-position body-b) 235 | (vec-rotate (squirl::damped-spring-anchor2 spring) 236 | (body-rotation body-b)))) 237 | (delta (vec- point-a point-b)) 238 | (ziggy (floor (/ (spring-stiffness spring) 10))) 239 | (width (/ ziggy 3))) 240 | (gl:point-size 8) 241 | (gl:with-primitives :points 242 | (gl:vertex (vec-x point-a) (vec-y point-a)) 243 | (gl:vertex (vec-x point-b) (vec-y point-b))) 244 | (gl:line-width 2) 245 | (gl:with-pushed-matrix 246 | (gl:translate (vec-x point-a) (vec-y point-a) 0) 247 | (gl:rotate (+ 90 (* (vec->angle delta) (/ 180 pi))) 0 0 1) 248 | (gl:scale width (/ (vec-length delta) ziggy) 1) 249 | (gl:with-primitive :line-strip 250 | (loop for i from 0 below ziggy do 251 | (gl:vertex -1 i) 252 | (gl:vertex 1 (+ i 1/2))) 253 | (gl:vertex 0 ziggy))))) 254 | 255 | (defun draw-vector (origin vector) 256 | (unless (vec-zerop vector) 257 | (gl:with-pushed-matrix 258 | (gl:translate (vec-x origin) (vec-y origin) 0) 259 | (gl:rotate (- (* (vec->angle vector) (/ 180 pi)) 90) 0 0 1) 260 | (gl:scale 10 (vec-length vector) 1) 261 | (gl:with-primitive :lines 262 | (gl:vertex 0 0) 263 | (gl:vertex 0 1) 264 | 265 | (gl:vertex 0 1) 266 | (gl:vertex 0.25 0.75) 267 | 268 | (gl:vertex 0 1) 269 | (gl:vertex -0.25 0.75))))) 270 | 271 | (defun draw-velocity (body) 272 | (gl:color 0 0 1) 273 | (gl:line-width 2) 274 | (draw-vector (body-position body) (body-velocity body))) 275 | 276 | (defun draw-force (body) 277 | (gl:color 0 1 0) 278 | (gl:line-width 2) 279 | (draw-vector (body-position body) (body-force body))) 280 | 281 | (defun draw-collision-normal (arbiter) 282 | (gl:color 1 0 0) 283 | (gl:line-width 2) 284 | (loop for contact in (squirl::arbiter-contacts arbiter) 285 | for normal = (squirl::contact-normal contact) 286 | for position = (squirl::contact-point contact) 287 | do (draw-vector position (vec* normal 10d0)))) 288 | 289 | ;;; 290 | ;;; Drawing the world. 291 | ;;; 292 | (defun set-body-point (body) 293 | (gl:vertex (vec-x (body-position body)) 294 | (vec-y (body-position body)))) 295 | 296 | (defun set-collision-points (arbiter) 297 | (loop for contact in (squirl::arbiter-contacts arbiter) 298 | for contact-position = (squirl::contact-point contact) 299 | do (gl:vertex (vec-x contact-position) (vec-y contact-position)))) 300 | 301 | (defun draw-world (world &key (line-thickness 1) 302 | draw-bb-p (draw-shapes-p t) (body-point-size 2) (collision-point-size 2) 303 | draw-force draw-velocity draw-collision-normal) 304 | (gl:line-width line-thickness) 305 | (when draw-shapes-p 306 | (map-world #'draw-body world)) 307 | (when draw-bb-p 308 | (map-world #'draw-bbox world)) 309 | ;; draw constraints 310 | (gl:color 0.5 1 0.5) 311 | (map nil #'draw-constraint (world-constraints world)) 312 | (when (> body-point-size 0) 313 | (gl:point-size body-point-size) 314 | (gl:with-primitives :points 315 | (apply #'gl:color *line-color*) 316 | (map-world #'set-body-point world))) 317 | (when (> collision-point-size 0) 318 | (gl:point-size collision-point-size) 319 | (gl:with-primitives :points 320 | (apply #'gl:color *collision-color*) 321 | (map nil #'set-collision-points (squirl::world-arbiters world)))) 322 | (when draw-collision-normal 323 | (map nil #'draw-collision-normal (squirl::world-arbiters world))) 324 | (when draw-velocity 325 | (map-world #'draw-velocity world)) 326 | (when draw-force 327 | (map-world #'draw-force world))) 328 | -------------------------------------------------------------------------------- /demo/squirl-demo.lisp: -------------------------------------------------------------------------------- 1 | (defpackage #:squirl-demo 2 | (:use :cl :squirl :squirl.utils) 3 | (:export :run-all-demos)) 4 | (in-package :squirl-demo) 5 | 6 | (defparameter *sleep-ticks* 16) 7 | 8 | (defvar *demos* nil) 9 | (defvar *current-demo* nil) 10 | 11 | (defvar *key-up* nil) 12 | (defvar *key-down* nil) 13 | (defvar *key-left* nil) 14 | (defvar *key-right* nil) 15 | 16 | (defvar *arrow-direction* +zero-vector+) 17 | 18 | (defvar *aa-enabled-p* nil) 19 | 20 | (defparameter *dt-threshold* 0.01) 21 | ;;; 22 | ;;; Utils 23 | ;;; 24 | (defun now () 25 | (/ (get-internal-real-time) internal-time-units-per-second)) 26 | 27 | (defun time-difference (time-before) 28 | "Checks the difference between the internal-time provided and the current time. 29 | Returns both the difference in time and the current-time used in the computation" 30 | (let* ((time-now (now)) 31 | (difference (- time-now time-before))) 32 | (if (minusp difference) 33 | 0 ; just in case 34 | (values (- time-now time-before) 35 | time-now)))) 36 | 37 | (defparameter *fps-sample-size* 10) 38 | (let (last-frame 39 | (fps-stack) 40 | (frames 0) 41 | (cumulative-mean 0)) 42 | (defun notify-frame () 43 | (when (> (length fps-stack) *fps-sample-size*) 44 | (setf (cdr (last fps-stack 2)) nil)) 45 | (let ((now (now))) 46 | (when last-frame 47 | (let ((time-delta (- now last-frame))) 48 | (unless (zerop time-delta) 49 | (push (/ time-delta) 50 | fps-stack) 51 | (setf cumulative-mean (/ (+ (last-fps) 52 | (* frames cumulative-mean)) 53 | (1+ frames))) 54 | (incf frames)))) 55 | (setf last-frame now))) 56 | (defun notify-unpause () 57 | (setf last-frame (now))) 58 | (defun last-fps () 59 | (first fps-stack)) 60 | (defun mean-fps () 61 | (when fps-stack 62 | (/ (reduce #'+ fps-stack) (length fps-stack)))) 63 | (defun cumulative-mean-fps () 64 | cumulative-mean) 65 | (defun reset-cumulative-mean-fps () 66 | "Restart calculating the cumulative mean relative to the current frame." 67 | (setf frames 0) 68 | (setf cumulative-mean 0))) 69 | 70 | ;;; 71 | ;;; Demo class 72 | ;;; 73 | (defclass demo () 74 | ((name :initarg :name :accessor demo-name) 75 | (pausedp :initform nil :accessor pausedp) 76 | (accumulator :initform 0 :accessor accumulator) 77 | (world :initarg :world :accessor world) 78 | (delta-time :initform 1 :accessor delta-time) 79 | (last-frame-time :initform (now) :accessor last-frame-time) 80 | (physics-timestep :initarg :physics-timestep 81 | :initform (float 1/120 1d0) :accessor physics-timestep) 82 | (mouse-joint :initform nil :accessor mouse-joint) 83 | (mouse-body :initarg :mouse-body :initform (make-body) :accessor mouse-body) 84 | (last-mouse-position :initform +zero-vector+ :accessor last-mouse-position) 85 | ;; drawing 86 | (line-thickness :initarg :line-thickness :initform 1 :accessor line-thickness) 87 | (draw-shapes-p :initarg :draw-shapes-p :initform t :accessor draw-shapes-p) 88 | (draw-bb-p :initarg :draw-bb-p :initform nil :accessor draw-bb-p) 89 | (draw-force-p :initarg :draw-force-p :initform nil :accessor draw-force-p) 90 | (draw-velocity-p :initarg :draw-velocity-p :initform nil :accessor draw-velocity-p) 91 | (body-point-size :initarg :body-point-size :initform 0 :accessor body-point-size) 92 | (collision-point-size :initarg :collision-point-size :initform 2 :accessor collision-point-size) 93 | (draw-collision-normal-p :initarg :draw-collision-normal-p :initform nil :accessor draw-collision-normal-p))) 94 | 95 | (defgeneric mouse-position (demo) 96 | (:method ((demo demo)) (body-position (mouse-body demo)))) 97 | (defgeneric (setf mouse-position) (new-pos demo) 98 | (:method (new-pos (demo demo)) (setf (body-position (mouse-body demo)) new-pos))) 99 | 100 | (defun update-time (demo) 101 | (with-slots (delta-time last-frame-time) demo 102 | (multiple-value-bind (new-dt now) 103 | (time-difference last-frame-time) 104 | (setf last-frame-time now 105 | delta-time new-dt)))) 106 | 107 | (defgeneric draw-demo (demo) 108 | (:method ((demo demo)) 109 | (with-slots (line-thickness draw-shapes-p draw-bb-p body-point-size collision-point-size draw-force-p draw-velocity-p draw-collision-normal-p) 110 | *current-demo* 111 | (draw-world (world *current-demo*) :draw-shapes-p draw-shapes-p 112 | :draw-bb-p draw-bb-p :line-thickness line-thickness 113 | :body-point-size body-point-size :collision-point-size collision-point-size 114 | :draw-force draw-force-p :draw-velocity draw-velocity-p 115 | :draw-collision-normal draw-collision-normal-p)))) 116 | 117 | (defgeneric update-demo (demo dt)) 118 | (defgeneric init-demo (demo)) 119 | (defgeneric grabbablep (actor) 120 | (:method (actor) (declare (ignore actor)) t) 121 | (:method ((actor (eql :not-grabbable))) nil)) 122 | 123 | (defun toggle-pause (demo) 124 | (if (pausedp demo) 125 | (progn (setf (pausedp demo) nil) 126 | (notify-unpause)) 127 | (setf (pausedp demo) t))) 128 | 129 | (defmethod update-demo :around ((demo demo) dt) 130 | (declare (ignore dt)) 131 | (unless (pausedp demo) 132 | (call-next-method))) 133 | 134 | (defmethod update-demo ((demo demo) dt) 135 | "The default method locks the update loop to 'realtime'. That is, it 136 | makes sure that the current world is updated by 1 time unit per second." 137 | (incf (accumulator demo) (if (> dt *dt-threshold*) *dt-threshold* dt)) 138 | (loop while (>= (accumulator demo) (physics-timestep demo)) 139 | do (world-step (world demo) (physics-timestep demo)) 140 | (decf (accumulator demo) (physics-timestep demo)))) 141 | 142 | ;;; 143 | ;;; Drawing the demos 144 | ;;; 145 | (defclass squirl-window (glut:window) 146 | () 147 | (:default-initargs :width 640 :height 480 :mode '(:double :rgba :multisample) 148 | :title "Squirl Demo App")) 149 | 150 | (defun draw-string (x y string) 151 | (gl:color 0 0 0) 152 | (gl:raster-pos x y) 153 | (glut:bitmap-string glut:+bitmap-helvetica-10+ string)) 154 | 155 | (defun draw-instructions () 156 | (let ((x -300) (y 220)) 157 | (draw-string x y (format nil 158 | "Controls:~@ 159 | #\\N chooses the next demo~@ 160 | #\\P chooses the previous demo~@ 161 | #\\Space toggles pause~@ 162 | #\\Return restarts the current demo~@ 163 | Use the mouse to grab objects~@ 164 | Arrow keys control some demos~@ 165 | #\\A toggles anti-aliasing~@ 166 | #\\[ and #\\] control the size of body points~@ 167 | #\\{ and #\\} control the size of collision points.~@ 168 | #\\V toggles velocity vectors~@ 169 | #\\F toggles force vectors~@ 170 | #\\C toggles collision normals")))) 171 | 172 | (defun draw-fps () 173 | (let ((x -300) (y 0)) 174 | (draw-string x y (format nil "Last FPS: ~7,2f~%Mean FPS: ~7,2f~%Cumulative Mean FPS:~7,2f" 175 | (last-fps) (mean-fps) (cumulative-mean-fps))))) 176 | 177 | (defun draw-pause-state () 178 | (when (pausedp *current-demo*) 179 | (draw-string -300 10 "SIMULATION PAUSED"))) 180 | 181 | (defmethod glut:idle ((w squirl-window)) 182 | (unless (pausedp *current-demo*) 183 | (notify-frame)) 184 | (update-time *current-demo*) 185 | (glut:post-redisplay)) 186 | 187 | (defmethod glut:display ((w squirl-window)) 188 | (gl:clear :color-buffer-bit) 189 | (draw-demo *current-demo*) 190 | (draw-fps) 191 | (draw-pause-state) 192 | (draw-instructions) 193 | (glut:swap-buffers) 194 | (let ((new-point (vec-lerp (last-mouse-position *current-demo*) 195 | (mouse-position *current-demo*) 1/4))) 196 | (setf (mouse-position *current-demo*) new-point 197 | (body-velocity (mouse-body *current-demo*)) (vec* (vec- new-point 198 | (last-mouse-position *current-demo*)) 199 | 60d0) 200 | (last-mouse-position *current-demo*) new-point) 201 | (update-demo *current-demo* (delta-time *current-demo*)))) 202 | 203 | (defun demo-title (demo) 204 | (concatenate 'string "Demo: " (demo-name demo))) 205 | 206 | (defun run-demo (demo-class) 207 | (let ((old-demo *current-demo*)) 208 | (reset-cumulative-mean-fps) 209 | (clear-color-hash) 210 | (setf *current-demo* (make-instance demo-class) 211 | (world *current-demo*) (init-demo *current-demo*)) 212 | (when old-demo 213 | (setf (mouse-position *current-demo*) (mouse-position old-demo) 214 | (last-mouse-position *current-demo*) (last-mouse-position old-demo))))) 215 | 216 | (defmethod glut:keyboard ((w squirl-window) key x y) 217 | (declare (ignore x y)) 218 | (when (upper-case-p key) 219 | (setf key (char-downcase key))) 220 | (case key 221 | (#\Esc (glut:destroy-current-window)) 222 | (#\Return (run-demo (class-of *current-demo*))) 223 | (#\Space (toggle-pause *current-demo*)) 224 | (#\n (run-demo (elt *demos* 225 | (mod (1+ (position (class-name (class-of *current-demo*)) *demos*)) 226 | (length *demos*))))) 227 | (#\p (run-demo (elt *demos* 228 | (mod (1- (position (class-name (class-of *current-demo*)) *demos*)) 229 | (length *demos*))))) 230 | (#\a (toggle-anti-aliasing)) 231 | (#\] (incf (body-point-size *current-demo*))) 232 | (#\[ (unless (<= (body-point-size *current-demo*) 0) 233 | (decf (body-point-size *current-demo*)))) 234 | (#\} (incf (collision-point-size *current-demo*))) 235 | (#\{ (unless (<= (collision-point-size *current-demo*) 0) 236 | (decf (collision-point-size *current-demo*)))) 237 | (#\v (setf (draw-velocity-p *current-demo*) (not (draw-velocity-p *current-demo*)))) 238 | (#\f (setf (draw-force-p *current-demo*) (not (draw-force-p *current-demo*)))) 239 | (#\c (setf (draw-collision-normal-p *current-demo*) (not (draw-collision-normal-p *current-demo*)))))) 240 | 241 | (defun toggle-anti-aliasing () 242 | (if *aa-enabled-p* 243 | (disable-anti-aliasing) 244 | (enable-anti-aliasing))) 245 | 246 | (defun disable-anti-aliasing () 247 | (gl:disable #+nil :polygon-smooth :line-smooth :point-smooth :blend :multisample) 248 | (setf *aa-enabled-p* nil)) 249 | 250 | (defun enable-anti-aliasing () 251 | (gl:enable #+nil :polygon-smooth :line-smooth :point-smooth :blend :multisample) 252 | (gl:blend-func :src-alpha :one-minus-src-alpha) 253 | (gl:hint :polygon-smooth-hint :nicest) 254 | (gl:hint :line-smooth-hint :nicest) 255 | #+nil (gl:hint :point-smooth-hint :nicest) 256 | (setf *aa-enabled-p* t)) 257 | 258 | (defun mouse-to-space (x y) 259 | (let ((model (gl:get-double :modelview-matrix)) 260 | (proj (gl:get-double :projection-matrix)) 261 | (view (gl:get-double :viewport))) 262 | (multiple-value-bind (mx my) 263 | (glu:un-project x (- (glut:get :window-height) y) 0 264 | :modelview model :projection proj :viewport view) 265 | (vec mx my)))) 266 | 267 | (defmethod glut:motion ((w squirl-window) x y) 268 | (setf (mouse-position *current-demo*) (mouse-to-space x y))) 269 | (defmethod glut:passive-motion ((w squirl-window) x y) 270 | (setf (mouse-position *current-demo*) (mouse-to-space x y))) 271 | 272 | (defmethod glut:mouse ((w squirl-window) button state x y) 273 | (if (eq button :left-button) 274 | (if (eq state :down) 275 | (let* ((point (mouse-to-space x y)) 276 | (shape (world-point-query-first (world *current-demo*) point))) 277 | (when (and shape (grabbablep (body-actor (shape-body shape)))) 278 | (let ((body (shape-body shape))) 279 | (setf (mouse-joint *current-demo*) (make-pivot-joint (mouse-body *current-demo*) body 280 | +zero-vector+ 281 | (world->body-local body point)) 282 | (squirl::constraint-max-force (mouse-joint *current-demo*)) 50000 283 | (squirl::constraint-bias-coefficient (mouse-joint *current-demo*)) 0.15) 284 | (world-add-constraint (world *current-demo*) (mouse-joint *current-demo*))))) 285 | (progn (world-remove-constraint (world *current-demo*) (mouse-joint *current-demo*)) 286 | (setf (mouse-joint *current-demo*) nil))))) 287 | 288 | (cffi:defcallback timercall :void ((value :int)) 289 | (declare (ignore value)) 290 | (glut:timer-func 16 (cffi:callback timercall) 0) 291 | (glut:post-redisplay)) 292 | 293 | (defun set-arrow-direction () 294 | (let ((x 0) (y 0)) 295 | (when *key-up* (incf y)) 296 | (when *key-down* (decf y)) 297 | (when *key-right* (incf x)) 298 | (when *key-left* (decf x)) 299 | (setf *arrow-direction* (vec x y)))) 300 | 301 | (defmethod glut:special ((w squirl-window) key x y) 302 | (declare (ignore x y)) 303 | (case key 304 | (:key-up (setf *key-up* t)) 305 | (:key-down (setf *key-down* t)) 306 | (:key-left (setf *key-left* t)) 307 | (:key-right (setf *key-right* t))) 308 | (set-arrow-direction)) 309 | 310 | (defmethod glut:special-up ((w squirl-window) key x y) 311 | (declare (ignore x y)) 312 | (case key 313 | (:key-up (setf *key-up* nil)) 314 | (:key-down (setf *key-down* nil)) 315 | (:key-left (setf *key-left* nil)) 316 | (:key-right (setf *key-right* nil))) 317 | (set-arrow-direction)) 318 | 319 | (defmethod glut:display-window :before ((w squirl-window)) 320 | (gl:clear-color 1 1 1 0) 321 | (gl:matrix-mode :projection) 322 | (gl:load-identity) 323 | (gl:ortho -320 320 -240 240 -1 1) 324 | (gl:translate 1/2 1/2 0) 325 | (gl:enable-client-state :vertex-array) 326 | (enable-anti-aliasing) 327 | #+nil(glut:timer-func 16 (cffi:callback timercall) 0)) 328 | 329 | (defun run-all-demos () 330 | (when *demos* 331 | (run-demo (nth (random (length *demos*)) *demos*)) 332 | (glut:display-window (make-instance 'squirl-window)) 333 | ;; this is a kludge around an apparent cl-glut bug. 334 | (setf glut::*glut-initialized-p* nil))) 335 | -------------------------------------------------------------------------------- /src/world.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (defvar *contact-persistence* 3) 5 | 6 | ;;; These names can be better 7 | (defparameter *default-iterations* 10) 8 | (defparameter *default-elastic-iterations* 0) 9 | (defparameter *initial-cell-size* 100d0) 10 | (defparameter *initial-count* 1000) 11 | (defparameter *initial-array-length* 4) 12 | 13 | (defgeneric collide (actor1 actor2 arbiter) 14 | (:method ((actor1 t) (actor2 t) (arbiter t)) t)) 15 | 16 | (defstruct (world 17 | (:constructor %make-world 18 | (&key iterations elastic-iterations gravity damping collision-callback))) 19 | ;; Number of iterations to use in the impulse solver to solve contacts. 20 | (iterations *default-iterations* :type fixnum) 21 | ;; Number of iterations to use in the impulse solver to solve elastic collisions. 22 | (elastic-iterations *default-elastic-iterations* :type fixnum) 23 | ;; Default gravity to supply when integrating rigid body motions. 24 | (gravity +zero-vector+ :type vec) 25 | ;; Default damping to supply when integrating rigid body motions. 26 | (damping 1d0 :type double-float) 27 | 28 | ;; Internal slots 29 | (timestamp 0 :type fixnum) ; Time stamp, incremented on every call to WORLD-STEP 30 | ;; Static and active shape spatial hashes 31 | (static-shapes (make-world-hash *initial-cell-size* *initial-count*) :type world-hash) 32 | (active-shapes (make-world-hash *initial-cell-size* *initial-count*) :type world-hash) 33 | ;; Static and active bodies 34 | (static-bodies (make-adjustable-vector *initial-array-length*) :type (vector t)) 35 | (active-bodies (make-adjustable-vector *initial-array-length*) :type (vector t)) 36 | ;; Active arbiters for the impulse solver. 37 | (arbiters (make-adjustable-vector *initial-array-length*) :type (vector t)) 38 | (contact-set (make-hash-set 0 #'arbiter-shapes-equal) :type hash-set) ; Persistent contact set. 39 | ;; Constraints in the system. 40 | (constraints (make-adjustable-vector *initial-array-length*) :type (vector t)) 41 | arbitrator 42 | 43 | ;; Collision callback 44 | (collision-callback #'collide)) 45 | 46 | (defun make-world (&rest keys) 47 | (declare (dynamic-extent keys)) 48 | (let ((world (apply #'%make-world keys))) 49 | (with-place (|| world-) (contact-set timestamp arbiters) world 50 | (setf (world-arbitrator world) 51 | ;; Let us thank Scott Lembcke, who had to hunt bugs down and code solutions 52 | ;; in C, while we can simply port said solutions into CL. 53 | (lambda (shape1 shape2) 54 | ;; This is a kludge. It might break on new shape types. 55 | (when (or (and (poly-p shape1) (circle-p shape2)) 56 | (and (poly-p shape1) (segment-p shape2))) 57 | (rotatef shape1 shape2)) 58 | (when (collision-possible-p shape1 shape2) 59 | (awhen (collide-shapes shape1 shape2) 60 | (let ((arbiter (ensure-arbiter shape1 shape2 contact-set timestamp))) 61 | ;; This is also a kludge... got any better ideas? 62 | (setf (arbiter-shape-a arbiter) shape1 63 | (arbiter-shape-b arbiter) shape2) 64 | (vector-push-extend arbiter arbiters) 65 | (arbiter-inject arbiter it))))))) 66 | world)) 67 | 68 | (define-print-object (world) 69 | (format t "Iterations: ~a, Elastic iterations: ~a, Gravity: ~a, Body count: ~a" 70 | (world-iterations world) 71 | (world-elastic-iterations world) 72 | (world-gravity world) 73 | (+ (length (world-active-bodies world)) 74 | (length (world-static-bodies world))))) 75 | 76 | (defmacro defcollision (&body args) 77 | (multiple-value-bind (qualifiers lambda-list body) 78 | (parse-defmethod args) 79 | (destructuring-bind (arg-a arg-b arbiter) lambda-list 80 | (flet ((parse-specialized-arg (arg) 81 | (etypecase arg 82 | (symbol (values arg t)) 83 | (list (destructuring-bind (arg-name specializer) arg 84 | (values arg-name specializer)))))) 85 | (multiple-value-bind (actor-a spec-a) 86 | (parse-specialized-arg arg-a) 87 | (multiple-value-bind (actor-b spec-b) 88 | (parse-specialized-arg arg-b) 89 | (with-gensyms (cnm-sym nmp-sym) 90 | (if (equal spec-a spec-b) 91 | `(defmethod collide ,@qualifiers ,lambda-list ,@body) 92 | `(flet ((handler (,actor-a ,actor-b ,arbiter ,cnm-sym ,nmp-sym) 93 | ,@(pop-declarations body) 94 | (flet ((call-next-method (&rest cnm-args) 95 | (apply ,cnm-sym cnm-args)) 96 | (next-method-p () (funcall ,nmp-sym))) 97 | (declare (ignorable #'call-next-method #'next-method-p)) 98 | ,@body))) 99 | (defmethod collide ,@qualifiers (,arg-a ,arg-b ,arbiter) 100 | (handler ,actor-a ,actor-b ,arbiter #'call-next-method #'next-method-p)) 101 | (defmethod collide ,@qualifiers (,arg-b ,arg-a ,arbiter) 102 | (handler ,actor-a ,actor-b ,arbiter #'call-next-method #'next-method-p))))))))))) 103 | 104 | ;;; 105 | ;;; Body, Shape, and Joint Management 106 | ;;; 107 | 108 | (defun world-add-static-shape (world shape) 109 | (with-place (shape. shape-) (id bbox body) shape 110 | (assert shape.body) 111 | (shape-cache-data shape) 112 | (world-hash-insert (world-static-shapes world) 113 | shape shape.id shape.bbox))) 114 | 115 | (defun world-add-active-shape (world shape) 116 | (with-place (shape. shape-) (id bbox body) shape 117 | (assert shape.body) 118 | (world-hash-insert (world-active-shapes world) shape shape.id shape.bbox))) 119 | 120 | (defun world-add-body (world body) 121 | ;; FLET or MACROLET this up please 122 | (cond ((staticp body) 123 | (assert (not (find body (world-static-bodies world)))) 124 | (vector-push-extend body (world-static-bodies world)) 125 | (dolist (shape (body-shapes body)) 126 | (world-add-static-shape world shape))) 127 | (t (assert (not (find body (world-active-bodies world)))) 128 | (vector-push-extend body (world-active-bodies world)) 129 | (dolist (shape (body-shapes body)) 130 | (world-add-active-shape world shape)))) 131 | (setf (body-world body) world) 132 | body) 133 | 134 | (defun world-add-constraint (world constraint) 135 | (assert (not (find constraint (world-constraints world)))) 136 | (vector-push-extend constraint (world-constraints world)) 137 | constraint) 138 | 139 | (defun shape-removal-arbiter-reject (world shape) 140 | (delete-iff (world-arbiters world) 141 | (fun (with-place (arb. arbiter-) ((a shape-a) (b shape-b)) _ 142 | (and (not (eq shape arb.a)) (not (eq shape arb.b))))))) 143 | 144 | (defun world-remove-shape (world shape) 145 | (world-hash-remove (if (staticp (shape-body shape)) 146 | (world-static-shapes world) 147 | (world-active-shapes world)) 148 | shape (shape-id shape)) 149 | (shape-removal-arbiter-reject world shape)) 150 | 151 | (defun world-remove-body (world body) 152 | (map nil (fun (world-remove-shape world _)) (body-shapes body)) 153 | (if (staticp body) ; Needs more macrolet 154 | (deletef (world-static-bodies world) body) 155 | (deletef (world-active-bodies world) body))) 156 | 157 | (defun world-remove-constraint (world constraint) 158 | (deletef (world-constraints world) constraint)) 159 | 160 | ;;; 161 | ;;; Point Query Functions 162 | ;;; 163 | 164 | (defun world-point-query (function world point) 165 | (flet ((query-point-and-shape (point shape) 166 | (when (point-inside-shape-p shape point) 167 | (funcall function shape)))) 168 | (world-hash-point-query #'query-point-and-shape (world-active-shapes world) point) 169 | (world-hash-point-query #'query-point-and-shape (world-static-shapes world) point))) 170 | 171 | ;;; Unlike the C version, this actually returns the -first- shape 172 | ;;; encountered which matches the layers, groups, and point. It 173 | ;;; uses a functional RETURN-FROM rather than the pointer juggling 174 | ;;; from the C version, for speed and clarity. 175 | (defun world-point-query-first (world point) 176 | (world-point-query (fun (return-from world-point-query-first _)) 177 | world point)) 178 | 179 | ;;; 180 | ;;; Body Convenience Functions 181 | ;;; 182 | 183 | (defun map-world (function world) 184 | "Calls FUNCTION on each body in WORLD" 185 | (map nil function (world-static-bodies world)) 186 | (map nil function (world-active-bodies world))) 187 | 188 | (defun world-bodies (world) 189 | (with-place (|| world-) (active-bodies static-bodies) world 190 | (concatenate 'vector static-bodies active-bodies))) 191 | 192 | ;;; 193 | ;;; Segment Query Functions 194 | ;;; 195 | 196 | (defun world-shape-segment-query (function world start end &aux collisionp) 197 | (flet ((query-shape (shape) 198 | (prog1 1.0 199 | (when (segment-intersects-shape-p shape start end) 200 | (when function (funcall function shape 0d0 +zero-vector+)) 201 | (setf collisionp t))))) 202 | (world-hash-query-segment #'query-shape (world-static-shapes world) start end) 203 | (world-hash-query-segment #'query-shape (world-active-shapes world) start end) 204 | collisionp)) 205 | 206 | (defun world-shape-segment-query-first (world start end &aux first-shape min-ratio first-normal) 207 | (flet ((query-shape (shape) 208 | (multiple-value-bind (hitp ratio normal) 209 | (segment-intersects-shape-p shape start end) 210 | (when (and hitp (< ratio min-ratio)) 211 | (setf first-shape shape 212 | min-ratio ratio 213 | first-normal normal))))) 214 | (world-hash-query-segment #'query-shape (world-static-shapes world) start end) 215 | (world-hash-query-segment #'query-shape (world-active-shapes world) start end) 216 | (values first-shape min-ratio first-normal))) 217 | 218 | ;;; 219 | ;;; World Hash Management 220 | ;;; 221 | 222 | (defun resize-world-static-hash (world dimension count) 223 | (resize-world-hash (world-static-shapes world) (float dimension 1d0) count) 224 | (rehash-world-hash (world-static-shapes world))) 225 | 226 | (defun resize-world-active-hash (world dimension count) 227 | (resize-world-hash (world-active-shapes world) (float dimension 1d0) count)) 228 | 229 | (defun rehash-world-static-data (world) 230 | (map-world-hash #'shape-cache-data (world-static-shapes world)) 231 | (rehash-world-hash (world-static-shapes world))) 232 | 233 | ;;; 234 | ;;; Collision Detection Functions 235 | ;;; 236 | 237 | (defun collision-possible-p (shape1 shape2) 238 | (with-place (a. shape-) ((bb bbox) body group layers) shape1 239 | (with-place (b. shape-) ((bb bbox) body group layers) shape2 240 | (and (not (eq a.body b.body)) 241 | (bbox-intersects-p a.bb b.bb))))) 242 | 243 | ;;; 244 | ;;; Arbiter Frobbing Functions 245 | ;;; 246 | 247 | (defun filter-world-arbiters (world) 248 | "Filter arbiter list based on collisions." 249 | (delete-iff (world-arbiters world) 250 | (fun (let ((a (body-actor (shape-body (arbiter-shape-a _)))) 251 | (b (body-actor (shape-body (arbiter-shape-b _))))) 252 | (when (or a b) (not (funcall (world-collision-callback world) a b _))))))) 253 | 254 | (defun flush-arbiters (world) 255 | "Flush outdated arbiters." 256 | (with-place (|| world-) (timestamp contact-set arbiters) world 257 | (hash-set-delete-if (fun (> (- timestamp (arbiter-stamp _)) *contact-persistence*)) 258 | contact-set) 259 | (setf (fill-pointer arbiters) 0))) 260 | 261 | (defun ensure-arbiter (shape1 shape2 hash-set timestamp) 262 | (let* ((hash (hash-pair (shape-id shape1) (shape-id shape2))) 263 | (arbiter (hash-set-find-if (fun (arbiter-has-shapes-p _ shape1 shape2)) 264 | hash-set hash))) 265 | (if arbiter 266 | (prog1 arbiter (setf (arbiter-stamp arbiter) timestamp)) 267 | (hash-set-insert hash-set hash (make-arbiter nil shape1 shape2 timestamp))))) 268 | 269 | ;;; 270 | ;;; All-Important WORLD-STEP Function 271 | ;;; 272 | 273 | (defun resolve-collisions (world) 274 | "Resolves collisions between objects in WORLD." 275 | (with-place (|| world-) (active-shapes static-shapes arbiters arbitrator) world 276 | (map-world-hash #'shape-cache-data active-shapes) ; Pre-cache BBoxen 277 | ;; Detect collisions between active and static shapes. 278 | (map-world-hash (fun (world-hash-query arbitrator static-shapes _ (shape-bbox _))) 279 | active-shapes) 280 | ;; This seems to be detecting collisions between active shapes. 281 | (world-hash-query-rehash arbitrator active-shapes)) 282 | (filter-world-arbiters world)) 283 | 284 | (defun prestep-world (world dt dt-inv) 285 | (with-place (|| world-) (arbiters constraints) world 286 | ;; Prestep the arbiters 287 | (do-vector (arbiter arbiters) 288 | (arbiter-prestep arbiter dt-inv)) 289 | ;; Prestep the constraints 290 | (do-vector (constraint constraints) 291 | (pre-step constraint dt dt-inv)))) 292 | 293 | (defun apply-elastic-impulses (world) 294 | (with-place (|| world-) (arbiters constraints elastic-iterations) world 295 | (loop repeat elastic-iterations 296 | do (do-vector (arbiter arbiters) 297 | (arbiter-apply-impulse arbiter t)) 298 | (map nil #'apply-impulse constraints)))) 299 | 300 | (defun integrate-velocities (world dt &aux (damping (expt (/ (world-damping world)) (- dt)))) 301 | (with-place (|| world-) (active-bodies arbiters gravity) world 302 | ;; Apply gravity forces. 303 | (do-vector (body active-bodies) 304 | (body-update-velocity body gravity damping dt)) 305 | ;; Apply cached arbiter impulses. 306 | (map nil #'arbiter-apply-cached-impulse arbiters))) 307 | 308 | (defun solve-impulses (world) 309 | "Run the impulse solver, using the old-style elastic solver if elastic iterations are disabled" 310 | (with-place (|| world-) (iterations elastic-iterations arbiters constraints) world 311 | (loop with old-style-p = (zerop elastic-iterations) 312 | repeat iterations do 313 | (do-vector (arbiter arbiters) 314 | (arbiter-apply-impulse arbiter old-style-p)) 315 | (do-vector (constraint constraints) 316 | (apply-impulse constraint))))) 317 | 318 | (defun world-step (world timestep) 319 | "Step the physical state of WORLD by DT seconds." 320 | (assert (not (zerop timestep)) (world) "Cannot step ~A by 0" world) 321 | (let* ((dt (float timestep 0d0)) (dt-inv (/ dt))) 322 | (with-place (|| world-) (active-bodies active-shapes) world 323 | (flush-arbiters world) 324 | (do-vector (body active-bodies) 325 | (body-update-position body dt)) ; Integrate positions 326 | (resolve-collisions world) 327 | (prestep-world world dt dt-inv) 328 | (apply-elastic-impulses world) 329 | (integrate-velocities world dt) 330 | (solve-impulses world) 331 | (incf (world-timestamp world))))) 332 | -------------------------------------------------------------------------------- /src/collision.lisp: -------------------------------------------------------------------------------- 1 | ;;;; -*- Mode: Lisp; indent-tabs-mode: nil -*- 2 | (in-package :squirl) 3 | 4 | (define-constant +collision-slop+ 0.1 5 | "Amount of allowed penetration. Used to reduce vibrating contacts.") 6 | 7 | ;;; 8 | ;;; Collision resolution functions 9 | ;;; 10 | (defun circle-to-circle-query (p1 p2 r1 r2) 11 | (declare (optimize speed) (double-float r1 r2) (vec p1 p2)) 12 | (let* ((delta (vec- p2 p1)) 13 | (mindist (+ r1 r2)) 14 | (distsq (vec-length-sq delta))) 15 | (when (< distsq (* mindist mindist)) 16 | (let* ((dist (sqrt distsq))) 17 | (make-contact (vec+ p1 (vec* delta 18 | (+ 0.5d0 (maybe/ (- r1 (/ mindist 2)) 19 | dist)))) 20 | (vec* delta (maybe/ 1d0 dist)) ; Same as (vec-normalize delta) 21 | (- dist mindist)))))) 22 | 23 | (defun circle-to-segment (circle segment) 24 | (let* ((radius-sum (+ (circle-radius circle) (segment-radius segment))) 25 | (normal-distance (- (vec. (segment-trans-normal segment) 26 | (circle-transformed-center circle)) 27 | (vec. (segment-trans-a segment) 28 | (segment-trans-normal segment)))) 29 | (distance (- (abs normal-distance) radius-sum))) 30 | (unless (plusp distance) 31 | (let ((tangent-distance (- (vec-cross (segment-trans-normal segment) 32 | (circle-transformed-center circle)))) 33 | (tangent-distance-min (- (vec-cross (segment-trans-normal segment) 34 | (segment-trans-a segment)))) 35 | (tangent-distance-max (- (vec-cross (segment-trans-normal segment) 36 | (segment-trans-b segment))))) 37 | (cond 38 | ((< tangent-distance tangent-distance-min) 39 | (when (>= tangent-distance (- tangent-distance-min radius-sum)) 40 | (circle-to-circle-query (circle-transformed-center circle) 41 | (segment-trans-a segment) 42 | (circle-radius circle) 43 | (segment-radius segment)))) 44 | ((< tangent-distance tangent-distance-max) 45 | (let ((normal (if (minusp normal-distance) 46 | (segment-trans-normal segment) 47 | (vec- (segment-trans-normal segment))))) 48 | (make-contact (vec+ (circle-transformed-center circle) 49 | (vec* normal (+ (circle-radius circle) (/ distance 2)))) 50 | normal distance))) 51 | ((< tangent-distance (+ tangent-distance-max radius-sum)) 52 | (circle-to-circle-query (circle-transformed-center circle) 53 | (segment-trans-b segment) 54 | (circle-radius circle) 55 | (segment-radius segment))) 56 | (t nil)))))) 57 | 58 | ;;; This function has VERY HAIRY control flow. Frob with EXTREME caution. 59 | (defun find-min-separating-axis (poly1 poly2) 60 | (loop with msa 61 | with min-distance 62 | for axis across (poly-transformed-axes poly2) 63 | for distance = (poly-value-on-axis poly1 (poly-axis-normal axis) (poly-axis-distance axis)) 64 | never (plusp distance) 65 | when (or (null min-distance) (> distance min-distance)) 66 | do (setf msa axis 67 | min-distance distance) 68 | finally (return (values msa min-distance)))) 69 | 70 | (defun find-vertices (poly1 poly2 normal distance &aux contacts) 71 | "Add contacts for penetrating vertices" 72 | (declare (optimize speed) (vec normal)) 73 | (let ((-normal (vec- normal))) 74 | (do-vector ((i vertex) (poly-transformed-vertices poly1)) 75 | (when (partial-poly-contains-vertex-p poly2 vertex -normal) 76 | (push (make-contact vertex normal distance (hash-pair (shape-id poly1) i)) contacts)))) 77 | (do-vector ((i vertex) (poly-transformed-vertices poly2) contacts) 78 | (when (partial-poly-contains-vertex-p poly1 vertex normal) 79 | (push (make-contact vertex normal distance (hash-pair (shape-id poly2) i)) contacts))) 80 | contacts) 81 | 82 | (defun segment-value-on-axis (segment normal distance) 83 | (- (min (- (vec. normal (segment-trans-a segment)) (segment-radius segment)) 84 | (- (vec. normal (segment-trans-b segment)) (segment-radius segment))) 85 | distance)) 86 | 87 | (defun find-points-behind-segment (segment poly p-dist coefficient &aux contacts) 88 | "Identify vertices that have penetrated the segment." 89 | (let* ((segment-normal (segment-trans-normal segment)) 90 | (dta (vec-cross segment-normal (segment-trans-a segment))) 91 | (dtb (vec-cross segment-normal (segment-trans-b segment))) 92 | (normal (vec* segment-normal coefficient)) 93 | (threshhold (+ (* (vec. segment-normal (segment-trans-a segment)) 94 | coefficient) 95 | (segment-radius segment)))) 96 | (do-vector ((i vertex) (poly-transformed-vertices poly) contacts) 97 | (when (< (vec. vertex normal) threshhold) 98 | (let ((dt (vec-cross segment-normal vertex))) 99 | (when (>= dta dt dtb) 100 | (push (make-contact vertex normal p-dist (hash-pair (shape-id poly) i)) 101 | contacts))))) 102 | contacts)) 103 | 104 | ;;; This is complicated. Not gross, but just complicated. It needs to be simpler 105 | ;;; and/or commented, preferably both. 106 | (defun segment-to-poly (segment poly &aux contacts) 107 | (let* ((axes (poly-transformed-axes poly)) 108 | (segD (vec. (segment-trans-normal segment) 109 | (segment-trans-a segment))) 110 | (min-norm (- (poly-value-on-axis poly (segment-trans-normal segment) segD) 111 | (segment-radius segment))) 112 | (min-neg (- (poly-value-on-axis poly (vec- (segment-trans-normal segment)) (- segD)) 113 | (segment-radius segment)))) 114 | (unless (or (> min-neg 0) (> min-norm 0)) 115 | (let ((min-i 0) 116 | (poly-min (segment-value-on-axis segment 117 | (poly-axis-normal (aref axes 0)) 118 | (poly-axis-distance (aref axes 0))))) 119 | (unless (or (plusp poly-min) 120 | (do-vector ((i axis) axes) 121 | (with-place (axis. poly-axis-) (normal distance) axis 122 | (let ((distance (segment-value-on-axis segment axis.normal axis.distance))) 123 | (when (> distance 0) (return t)) 124 | (when (> distance poly-min) 125 | (setf poly-min distance 126 | min-i i)))))) 127 | (let* ((poly-normal (vec- (poly-axis-normal (aref axes min-i)))) 128 | (vertex-a (vec+ (segment-trans-a segment) 129 | (vec* poly-normal (segment-radius segment)))) 130 | (vertex-b (vec+ (segment-trans-b segment) 131 | (vec* poly-normal (segment-radius segment))))) 132 | (flet ((try-vertex (vertex i) 133 | (when (poly-contains-vertex-p poly vertex) 134 | (push (make-contact vertex poly-normal poly-min 135 | (hash-pair (shape-id segment) i)) 136 | contacts)))) 137 | (try-vertex vertex-a 0) 138 | (try-vertex vertex-b 1)) 139 | ;; "Floating point precision problems here. 140 | ;; This will have to do for now." 141 | (decf poly-min +collision-slop+) 142 | (when (or (>= min-norm poly-min) 143 | (>= min-neg poly-min)) 144 | (setf contacts 145 | (nconc contacts 146 | (if (> min-norm min-neg) 147 | (find-points-behind-segment segment poly min-norm 1d0) 148 | (find-points-behind-segment segment poly min-neg -1d0))))) 149 | ;; If no other collision points were found, try colliding endpoints. 150 | (if contacts contacts 151 | (flet ((try-endpoint (point vertex) 152 | (let ((collision (circle-to-circle-query 153 | point vertex (segment-radius segment) 0d0))) 154 | (when collision (return-from segment-to-poly (list collision)))))) 155 | (let ((vert-a (aref (poly-transformed-vertices poly) min-i)) 156 | (vert-b (aref (poly-transformed-vertices poly) 157 | (rem (1+ min-i) (length (poly-transformed-vertices poly)))))) 158 | (try-endpoint (segment-trans-a segment) vert-a) 159 | (try-endpoint (segment-trans-b segment) vert-a) 160 | (try-endpoint (segment-trans-a segment) vert-b) 161 | (try-endpoint (segment-trans-b segment) vert-b)))))))))) 162 | 163 | (defun circle-to-poly (circle poly) 164 | (let* ((axes (poly-transformed-axes poly)) 165 | (min-i 0) 166 | (min (- (vec. (poly-axis-normal (svref axes 0)) 167 | (circle-transformed-center circle)) 168 | (poly-axis-distance (svref axes 0)) 169 | (circle-radius circle)))) 170 | (when (loop 171 | for i from 0 172 | for axis across axes 173 | for distance = (- (vec. (poly-axis-normal axis) 174 | (circle-transformed-center circle)) 175 | (poly-axis-distance axis) 176 | (circle-radius circle)) 177 | when (> distance 0) return nil 178 | when (> distance min) 179 | do (setf min distance 180 | min-i i) 181 | finally (return t)) 182 | (let* ((normal (poly-axis-normal (svref axes min-i))) 183 | (a (aref (poly-transformed-vertices poly) min-i)) 184 | (b (aref (poly-transformed-vertices poly) 185 | (rem (1+ min-i) (length (poly-transformed-vertices poly))))) 186 | (dta (vec-cross normal a)) 187 | (dtb (vec-cross normal b)) 188 | (dt (vec-cross normal (circle-transformed-center circle)))) 189 | (cond 190 | ((< dt dtb) 191 | (circle-to-circle-query (circle-transformed-center circle) 192 | b (circle-radius circle) 0d0)) 193 | ((< dt dta) 194 | (make-contact (vec- (circle-transformed-center circle) 195 | (vec* normal 196 | (+ (circle-radius circle) 197 | (/ min 2)))) 198 | (vec- normal) min)) 199 | (t (circle-to-circle-query (circle-transformed-center circle) 200 | a (circle-radius circle) 0d0))))))) 201 | 202 | (defun poly-to-poly (poly1 poly2) 203 | ;; This is definitely returning contacts, and they look correct. 204 | ;; The problem is elsewhere. 205 | (multiple-value-bind (msa1 min1) (find-min-separating-axis poly2 poly1) 206 | (multiple-value-bind (msa2 min2) (find-min-separating-axis poly1 poly2) 207 | (when (and msa1 msa2) 208 | (if (> min1 min2) 209 | (find-vertices poly1 poly2 (poly-axis-normal msa1) min1) 210 | (find-vertices poly1 poly2 (vec- (poly-axis-normal msa2)) min2)))))) 211 | 212 | (defun closest-point-on-segment (segment point &aux 213 | (a (segment-trans-a segment)) 214 | (line-vec (vec- (segment-trans-b segment) a))) 215 | (vec+ a (vec* line-vec (clamp (/ (vec. (vec- point a) line-vec) 216 | (vec. line-vec line-vec)) 217 | 0d0 1d0)))) 218 | 219 | (defun segment-intersection (a b &aux 220 | (a-a (segment-trans-a a)) 221 | (a-b (segment-trans-b a)) 222 | (b-a (segment-trans-a b)) 223 | (b-b (segment-trans-b b))) 224 | ;; Based on http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline2d/ 225 | (with-vecs (a-a a-b b-a b-b) 226 | (let ((x-factor-num (- (* (- b-b.x b-a.x) (- a-a.y b-a.y)) 227 | (* (- a-a.x b-a.x) (- b-b.y b-a.y)))) 228 | (y-factor-num (- (* (- a-b.x b-a.x) (- a-a.y b-a.y)) 229 | (* (- a-a.x b-a.x) (- a-b.y a-a.y)))) 230 | (denom (- (* (- a-b.x a-a.x) (- b-b.y b-a.y)) 231 | (* (- b-b.x b-a.x) (- a-b.y a-a.y))))) 232 | (cond 233 | ((= 0 denom x-factor-num y-factor-num) ; Coincident 234 | (vec* (vec- (segment-center a) (segment-center b)) 0.5d0)) 235 | ((= 0 denom) nil) ; Parallel 236 | (t (let* ((intersection (vec+ a-a 237 | (vec (* (/ x-factor-num denom) 238 | (- a-b.x a-a.x)) 239 | (* (/ y-factor-num denom) 240 | (- a-b.y a-a.y))))) 241 | (delta-a (vec- intersection a-a)) 242 | (delta-b (vec- intersection b-a)) 243 | (vec-a (vec- a-b a-a)) 244 | (vec-b (vec- b-b b-a))) 245 | (when (and (< (vec-length-sq delta-a) 246 | (vec-length-sq vec-a)) 247 | (< (vec-length-sq delta-b) 248 | (vec-length-sq vec-b)) 249 | ;; Make sure we're going along, not away from, the segment. 250 | (< (abs (- (vec->angle vec-a) 251 | (vec->angle delta-a))) 252 | pi) 253 | (< (abs (- (vec->angle vec-b) 254 | (vec->angle delta-b))) 255 | pi)) 256 | intersection))))))) 257 | 258 | (defun segment-center (segment) 259 | (vec* (vec+ (segment-trans-a segment) (segment-trans-b segment)) 260 | 0.5d0)) 261 | 262 | (defun segment-to-segment (a b &aux 263 | (end-a-a (segment-trans-a a)) 264 | (end-a-b (segment-trans-b a)) 265 | (end-b-a (segment-trans-a b)) 266 | (end-b-b (segment-trans-b b)) 267 | (radius-a (segment-radius a)) 268 | (radius-b (segment-radius b))) 269 | (if (= 0 radius-a radius-b) 270 | (progn 271 | (awhen (segment-intersection a b) 272 | (let ((delta (vec- (segment-center b) (segment-center a)))) 273 | (list (make-contact it (vec-normalize delta) (vec-length delta)))))) 274 | (let (contacts) 275 | (awhen (circle-to-circle-query 276 | (closest-point-on-segment a end-b-a) end-b-a 277 | radius-a radius-b) 278 | (push it contacts)) 279 | (awhen (circle-to-circle-query 280 | (closest-point-on-segment a end-b-b) end-b-b 281 | radius-a radius-b) 282 | (push it contacts)) 283 | (when (< (length contacts) 2) 284 | (awhen (circle-to-circle-query 285 | (closest-point-on-segment b end-a-a) end-a-a 286 | radius-b radius-a) 287 | (setf (contact-normal it) (vec- (contact-normal it))) 288 | (push it contacts)) 289 | (when (< (length contacts) 2) 290 | (awhen (circle-to-circle-query 291 | (closest-point-on-segment b end-a-b) end-a-b 292 | radius-b radius-a) 293 | (setf (contact-normal it) (vec- (contact-normal it))) 294 | (push it contacts)))) 295 | contacts))) 296 | 297 | ;;; 298 | ;;; Generic function 299 | ;;; 300 | (defgeneric collide-shapes (a b) 301 | (:documentation "Collide shapes A and B together!") 302 | (:method ((shape-1 circle) (shape-2 circle)) 303 | (ensure-list (circle-to-circle-query (circle-transformed-center shape-1) 304 | (circle-transformed-center shape-2) 305 | (circle-radius shape-1) 306 | (circle-radius shape-2)))) 307 | (:method ((segment segment) (circle circle)) 308 | (ensure-list (circle-to-segment circle segment))) 309 | (:method ((circle circle) (segment segment)) 310 | (ensure-list (circle-to-segment circle segment))) 311 | (:method ((segment segment) (poly poly)) 312 | (segment-to-poly segment poly)) 313 | (:method ((poly poly) (segment segment)) 314 | (segment-to-poly segment poly)) 315 | (:method ((circle circle) (poly poly)) 316 | (ensure-list (circle-to-poly circle poly))) 317 | (:method ((poly poly) (circle circle)) 318 | (ensure-list (circle-to-poly circle poly))) 319 | (:method ((poly1 poly) (poly2 poly)) 320 | (poly-to-poly poly1 poly2)) 321 | (:method ((seg1 segment) (seg2 segment)) 322 | (segment-to-segment seg1 seg2))) 323 | 324 | --------------------------------------------------------------------------------