luv

Workshop wiki

physics.lisp

luvcraft/physics.lisp

system luvcraft/core · 117 definitions · on GitHub

Rigid spheres in the block world: bodies, contacts, and the soft-step solver that moves them.

#G7Q3XR is the design this grew from and the Box3D field notes (#W2M9FJ, #S5K3WM, #R7F2QH, #G3W7KD) are the landmark it steers by. The shape of the thing is the seam #K2F6WD asks for: CLOS objects and generic functions own the world, its parameters, and its events; the bodies and contacts themselves are rows in generated columnar buffers, and every per-substep loop is a closed loop over borrowed specialized arrays.

What is here, in the order the step runs it:

  • Bodies are spheres. A body has a stable handle (an index into an id

table plus a generation, #N7Q4XS) and lives as one row of whichever SET currently holds it: the awake set the step iterates, or the sleeping set it does not. Sleep is relocation, not a flag; the id table is the forwarding address every relocation patches (#B5N8JT).

  • Terrain is the static tree (#D9W4CH): a sphere asks the voxel lattice

directly which exposed faces it is near, through a probe that keeps one chunk's storage borrowed between neighbouring questions. Moving things that are not spheres -- the player, an animal -- enter as KINEMATIC BOXES the client posts each step.

  • Body–body pairs come from a uniform hash grid over both sets. An

awake body which is moving wakes a sleeping one it reaches; one which is merely resting on it leans on it as if it were terrain.

  • Contacts are persistent rows keyed by their pair (#C6J9RW), found by

hash each step, pruned when their pair separates. What persists is the accumulated impulse -- the warm start -- and the touching state the begin/end events derive from.

  • Each step the contacts are COLOURED so that no two in one colour

share a dynamic body (#G3W7KD), and packed in colour order into a constraint buffer. That disjointness is what lets four contacts of a colour be solved in one f32.4 lane group (PHYSICS-SIMD.LISP) with no gather or scatter ever colliding. Static contacts take the highest colours so they are solved last in every iteration.

  • The solver is the Soft Step skeleton at its minimum: substeps of

integrate velocities, warm start, solve with bias, integrate positions, relax without bias; then restitution once, then the impulses are stored back on their contacts.

  • Events are buffered facts polled after the step (#F8H3PW): a contact

began or ended, a body hit something hard, expired, slept, or woke.

Everything numeric here is single-float, and every column is a SIMPLE-ARRAY SINGLE-FLOAT (*) or of fixnums, so that the scalar reference kernels and the four-wide kernels read the same storage. Reproducibility is of the same-image, fixed-code kind #S6T2MV settles on: the step's order is deterministic and a state hash can police it, and no more.

in-package#:luvcraft

--------------------------------------------------------------------- Parameters. Box3D's constants, in cells and seconds; each one is a special so it can be turned while a pile is settling.

defparameter*physics-gravity*-24.0"Gravitational acceleration, cells per second squared, along Y."
defparameter*physics-substeps*4"Substeps per step: the Soft Step loop runs this many times."
defparameter*physics-contact-hertz*30.0"Contact stiffness as a frequency; zero would be rigid."
defparameter*physics-contact-damping-ratio*10.0"Contact softness damping ratio."
defparameter*physics-contact-push-max-velocity*3.0"The most a soft contact will push overlapping bodies apart, cells/s."
defparameter*physics-linear-slop*0.005"How much overlap a contact tolerates before it pushes, in cells."
defparameter*physics-speculative-distance*0.02"How far apart two things may be and still have a contact between them."
defparameter*physics-restitution-threshold*1.0"Approach speed below which nothing bounces, cells/s."
defparameter*physics-sleep-speed*0.05"Below this speed a body accumulates sleep time, cells/s."
defparameter*physics-sleep-seconds*0.5"How long a body must be slow before it goes to sleep."
defparameter*physics-wake-speed*0.2"An awake body faster than this wakes the sleeping bodies it reaches."
defparameter*physics-hit-speed*2.0"Approach speed above which a contact reports a hit event, cells/s."
defparameter*physics-max-colors*24"How many disjoint constraint colours to try before the overflow colour."
defparameter*physics-terrain-margin*0.5

How far past its radius a body looks for terrain and boxes, in cells. Wide enough that a fast body's contacts exist a step before it lands.

--------------------------------------------------------------------- Handles. A body handle is a fixnum: the id-table index shifted up sixteen bits over a generation. A destroyed body's slot is reused with the next generation, so a stale handle answers BODY-ALIVE-P with NIL rather than naming whatever took its place. #N7Q4XS

defunphysics-handle-index
handle
declare
fixnumhandle
defunphysics-handle-generation
handle
declare
fixnumhandle
defunmake-physics-handle
indexgeneration
declare
fixnumindexgeneration
defconstant+physics-set-free+0"An id-table slot naming no body."
defconstant+physics-no-body+-1"The local index that means 'no body': the static side of a contact."

The id table: three parallel fixnum columns plus a free list. SET says which body set holds the body and LOCAL where in it; GENERATION is the handle's, incremented on every reuse of the slot.

defstruct
physics-id-table
:constructor%make-physics-id-table
set
make-array64:element-type'fixnum:initial-element0
:type
simple-arrayfixnum
local
make-array64:element-type'fixnum:initial-element0
:type
simple-arrayfixnum
generation
make-array64:element-type'fixnum:initial-element0
:type
simple-arrayfixnum
free
make-array16:element-type'fixnum:fill-pointer0:adjustablet
next0:typefixnum
defunmake-physics-id-table
%make-physics-id-table
defunphysics-id-table-grow
tableminimum
let*
old
physics-id-table-settable
capacity
maxminimum
flet
grow
column
let
new
make-arraycapacity:element-type'fixnum:initial-element0
replacenewcolumn
new
setf
physics-id-table-settable
grow
physics-id-table-settable
physics-id-table-localtable
grow
physics-id-table-localtable
physics-id-table-generationtable
grow
physics-id-table-generationtable
table
defunphysics-id-table-allocate
tablesetlocal

Claim an id slot for a body now at local in set; return its handle.

declare
fixnumsetlocal
let*
free
physics-id-table-freetable
index
if
plusp
fill-pointerfree
vector-popfree
let
next
physics-id-table-nexttable
when
>=next
length
physics-id-table-settable
setf
physics-id-table-nexttable
1+next
next
declare
fixnumindex
setf
aref
physics-id-table-settable
index
set
aref
physics-id-table-localtable
index
local
make-physics-handleindex
aref
physics-id-table-generationtable
index
defunphysics-id-table-release
tableindex

Give index's slot back; the next body to take it gets a new generation.

declare
fixnumindex
setf
aref
physics-id-table-settable
index
+physics-set-free+
aref
physics-id-table-generationtable
index
logand
1+
aref
physics-id-table-generationtable
index
+physics-handle-generation-mask+
vector-push-extendindex
physics-id-table-freetable
table
defunphysics-id-table-handle-live-p
tablehandle
declare
fixnumhandle
let
and
<index
physics-id-table-nexttable
/=
aref
physics-id-table-settable
index
+physics-set-free+
=
aref
physics-id-table-generationtable
index

--------------------------------------------------------------------- Body sets. One generated columnar layout serves both the awake and the sleeping set; a sleeping body simply keeps zero velocity in columns nobody iterates. The X Y Z here are absolute; DX DY DZ accumulate the substeps' motion within one step and are folded into X Y Z at its end, so that a contact's separation is a small delta added to a base value and a static side needs no state (#S5K3WM). Orientation is only for the eye: a sphere's contact geometry does not turn with it.

records:define-columnar-buffer
physics-body-columns:quantities
xyz
:quantity:world-position:unit:cell:tensor-order1
vxvyvz
:quantity:world-velocity:unit
:cell1
:second-1
:tensor-order1

The body's handle, so a moved row can patch its id-table entry.

handle0:typefixnum
x0f0:typesingle-float
y0f0:typesingle-float
z0f0:typesingle-float
dx0f0:typesingle-float
dy0f0:typesingle-float
dz0f0:typesingle-float
vx0f0:typesingle-float
vy0f0:typesingle-float
vz0f0:typesingle-float

Angular velocity, radians per second, world axes.

wx0f0:typesingle-float
wy0f0:typesingle-float
wz0f0:typesingle-float

Orientation as a unit quaternion (x y z w), for the renderer.

qx0f0:typesingle-float
qy0f0:typesingle-float
qz0f0:typesingle-float
qw1f0:typesingle-float
radius0.25f0:typesingle-float
inverse-mass1f0:typesingle-float

A solid sphere: 1 / (2/5 m r^2).

inverse-inertia1f0:typesingle-float
restitution0.3f0:typesingle-float
friction0.5f0:typesingle-float
rolling-resistance0.01f0:typesingle-float

Linear damping, per second: air drag, or something thicker.

damping0.05f0:typesingle-float

What the body looks like and is: an index into the client's palette of body kinds. The physics never reads it.

kind0:typefixnum

See PHYSICS-BODY-... below.

flags0:typefixnum

Seconds left to live; negative means immortal.

lifetime-1f0:typesingle-float

How long the body has been slow, toward sleep.

sleep-time0f0:typesingle-float
defconstant+physics-body-collides-with-bodies+1"Set on bodies that meet other bodies; a spray droplet does not."
defconstant+physics-body-hit-report+4"Set on bodies whose hard contacts should be reported as hit events."
defmacrodefine-columnar-remove-swap
namebuffer-type

Define (name BUFFER INDEX): move BUFFER's last row into INDEX and shrink.

Return the row index that was moved into INDEX, or NIL when INDEX was the last row and nothing had to move. The caller must then patch whatever pointed at the moved row: this is the forwarding-address discipline of #W2M9FJ, kept explicit at every call. Every lane is moved through its precise array type, so no float is boxed on the way.

let*
lanes
records:columnar-layout-definition-lanesdefinition
length-reader
intern
formatnil"~A-LENGTH"buffer-type
symbol-packagebuffer-type
flet
lane-form
lane
`
the
simple-array,
upgraded-array-element-type
,
intern
formatnil"~A-~A-LANE"buffer-type
records:columnar-lane-definition-namelane
symbol-packagebuffer-type
buffer
`
defun,name
bufferindex
declare
fixnumindex
optimize
speed3
safety1
let
last
1-
,length-readerbuffer
declare
fixnumlast
unless
<=0indexlast
error"Row ~D is not in ~S."indexbuffer
unless
=indexlast
,@
loopforlaneinlanescollect`
let
lane,
lane-formlane
setf
areflaneindex
areflanelast
,@
loopforlaneinlaneswhen
records:columnar-lane-definition-clear-on-remove-plane
collect`
setf
aref,
lane-formlane
last
,
records:columnar-lane-definition-initial-elementlane
setf
,length-readerbuffer
last
if
=indexlast
nillast
defmacrodefine-columnar-copy-row
namebuffer-type

Define (name SOURCE INDEX DESTINATION): push SOURCE's row INDEX onto DESTINATION and return its new index there.

let*
lanes
records:columnar-layout-definition-lanesdefinition
push-name
intern
formatnil"~A-PUSH"buffer-type
symbol-packagebuffer-type
length-reader
intern
formatnil"~A-LENGTH"buffer-type
symbol-packagebuffer-type
`
defun,name
sourceindexdestination
declare
fixnumindex
optimize
speed3
safety1
,push-namedestination,@
loopforlaneinlanescollect`
aref
the
simple-array,
upgraded-array-element-type
,
intern
formatnil"~A-~A-LANE"buffer-type
records:columnar-lane-definition-namelane
symbol-packagebuffer-type
source
index
1-
,length-readerdestination
defmacro%lane
buffer-typebufferlane

buffer's lane array with its precise specialized type, for the loops that borrow one lane at a time rather than a whole row schema.

let*
definition-lane
or
error"There is no ~S lane in ~S."lanebuffer-type
`
the
simple-array,
upgraded-array-element-type
,
intern
formatnil"~A-~A-LANE"buffer-typelane
symbol-packagebuffer-type
,buffer

--------------------------------------------------------------------- Kinematic boxes: the axis-aligned boxes of things that are not spheres and are moved by their own minds -- the player, an animal. The client posts them afresh every step; a body that meets one is pushed as if by terrain moving at the box's velocity, and the box feels nothing. OWNER is the client's tag, handed back in hit events.

records:define-columnar-bufferphysics-box-columns
min-x0f0:typesingle-float
min-y0f0:typesingle-float
min-z0f0:typesingle-float
max-x0f0:typesingle-float
max-y0f0:typesingle-float
max-z0f0:typesingle-float
vx0f0:typesingle-float
vy0f0:typesingle-float
vz0f0:typesingle-float
ownernil:typet:clear-on-removet

--------------------------------------------------------------------- Contacts. One row per pair within speculative distance, persistent across steps so its impulses can warm-start the next solve. KEY names the pair; KIND says what the other side is; LAST-STEP is when the pair was last within reach, so pruning is one comparison per row.

records:define-columnar-bufferphysics-contact-columns
key0:typefixnum
kind0:typefixnum

Handles, never local indices: rows move, handles do not.

handle-a0:typefixnum
handle-b0:typefixnum

For a box contact, the box's owner, for hit events.

ownernil:typet:clear-on-removet
last-step0:typefixnum
touching0:typefixnum

The accumulated impulses this pair carries from step to step.

normal-impulse0f0:typesingle-float
tangent-impulse-10f0:typesingle-float
tangent-impulse-20f0:typesingle-float
rolling-impulse-x0f0:typesingle-float
rolling-impulse-y0f0:typesingle-float
rolling-impulse-z0f0:typesingle-float

The step's manifold: one point. The normal points from A toward the other side; P is the contact point on the other side, in the world; KV is the other side's velocity when it is a kinematic box.

nx0f0:typesingle-float
ny0f0:typesingle-float
nz0f0:typesingle-float
px0f0:typesingle-float
py0f0:typesingle-float
pz0f0:typesingle-float
kvx0f0:typesingle-float
kvy0f0:typesingle-float
kvz0f0:typesingle-float
separation0f0:typesingle-float

The per-step constraint buffer: the contacts that will be solved, written in colour order (#G3W7KD) with everything the solver iterates over precomputed, so that a substep touches only these lanes and the awake body columns. A is always a dynamic body; B is a dynamic body, or the static dummy row past the end of the awake set. Anchors are from the body centres to the contact point. KV is the velocity of a kinematic other side (zero for terrain and bodies).

records:define-columnar-bufferphysics-constraint-columns

The persistent contact row whose impulses this constraint carries.

contact0:typefixnum
body-a0:typefixnum
body-b0:typefixnum
nx0f0:typesingle-float
ny0f0:typesingle-float
nz0f0:typesingle-float
t1x0f0:typesingle-float
t1y0f0:typesingle-float
t1z0f0:typesingle-float
t2x0f0:typesingle-float
t2y0f0:typesingle-float
t2z0f0:typesingle-float
rax0f0:typesingle-float
ray0f0:typesingle-float
raz0f0:typesingle-float
rbx0f0:typesingle-float
rby0f0:typesingle-float
rbz0f0:typesingle-float
kvx0f0:typesingle-float
kvy0f0:typesingle-float
kvz0f0:typesingle-float

Separation at the start of the step, already less the linear slop.

separation0f0:typesingle-float
normal-mass0f0:typesingle-float
tangent-mass0f0:typesingle-float
rolling-mass0f0:typesingle-float
restitution0f0:typesingle-float
friction0f0:typesingle-float
rolling-resistance0f0:typesingle-float

The approach speed before solving, which restitution answers to.

relative-velocity0f0:typesingle-float

The normal impulses of every substep summed: restitution and hit events only act on contacts that actually pushed.

total-normal-impulse0f0:typesingle-float

The softness this contact solves with (#R7F2QH); a static contact is stiffer than one between bodies. BIAS-RATE already carries MASS-SCALE.

bias-rate0f0:typesingle-float
mass-scale1f0:typesingle-float
impulse-scale0f0:typesingle-float
normal-impulse0f0:typesingle-float
tangent-impulse-10f0:typesingle-float
tangent-impulse-20f0:typesingle-float
rolling-impulse-x0f0:typesingle-float
rolling-impulse-y0f0:typesingle-float
rolling-impulse-z0f0:typesingle-float

--------------------------------------------------------------------- Events: what the step found out, for the client to poll (#F8H3PW). A begin or end names two handles (the second is PHYSICS-NO-BODY for terrain); a hit carries the contact point and approach speed; the rest name one body. OWNER is a box contact's owner.

records:define-columnar-bufferphysics-event-columns
kind:none:typekeyword
handle-a0:typefixnum
handle-b0:typefixnum
ownernil:typet:clear-on-removet
x0f0:typesingle-float
y0f0:typesingle-float
z0f0:typesingle-float
speed0f0:typesingle-float

--------------------------------------------------------------------- The world.

defclassphysics-world
ids:initform:readerphysics-world-ids
awake:initform
make-physics-body-columns:capacity256
:readerphysics-world-awake
sleeping:initform
make-physics-body-columns:capacity64
:readerphysics-world-sleeping
boxes:initform
make-physics-box-columns:capacity16
:readerphysics-world-boxes
contacts:initform
make-physics-contact-columns:capacity512
:readerphysics-world-contacts

Contact key -> contact row. Patched on every swap-remove.

contact-index:initform
make-hash-table:test#'eql
:readerphysics-world-contact-index
constraints:initform
make-physics-constraint-columns:capacity512
:readerphysics-world-constraints

Where each colour's constraints begin in CONSTRAINTS; one more entry than colours, the overflow colour last.

color-starts:initform
make-array:element-type'fixnum:initial-element0
:accessorphysics-world-color-starts

The colouring scratch: one bit-vector of awake bodies per colour, and the colour each contact was given this step.

color-bits:initform
make-array:initial-elementnil
:accessorphysics-world-color-bits
contact-colors:initform
make-array512:element-type'fixnum:initial-element0
:accessorphysics-world-contact-colors
events:initform
make-physics-event-columns:capacity64
:readerphysics-world-events
grid:initform
make-physics-grid
:readerphysics-world-grid
terrain:initarg:terrain:initformnil:accessorphysics-world-terrain:documentation"The block world the bodies collide with, or NIL."
probe:initform
make-terrain-probe
:readerphysics-world-probe
step-count:initform0:accessorphysics-world-step-count
step-seconds:initform
/1f060f0
:accessorphysics-world-step-seconds

The kernel family this world solves with; see PHYSICS-SIMD.LISP.

kernels:initform:scalar:accessorphysics-world-kernels

What the last step cost, so it can be read off the world.

last-step-real-seconds:initform0d0:accessorphysics-world-last-step-real-seconds
last-step-contact-count:initform0:accessorphysics-world-last-step-contact-count
last-step-color-count:initform0:accessorphysics-world-last-step-color-count

Contact rows made and dropped over the world's life: how much the pairs churn, which is what the hash and the buffer pay for.

contacts-made:initform0:accessorphysics-world-contacts-made
contacts-dropped:initform0:accessorphysics-world-contacts-dropped
:documentation

Every sphere the block world is simulating, and how they touch.

defmethodprint-object
stream
print-unreadable-object
worldstream:typet
formatstream"~D awake, ~D asleep, ~D contacts, step ~D, ~A"
physics-body-columns-length
physics-world-awakeworld
physics-body-columns-length
physics-world-sleepingworld
physics-contact-columns-length
physics-world-contactsworld
physics-world-step-countworld
physics-world-kernelsworld
defunmake-physics-world
&keyterrain
kernels:fastest
let
world
make-instance'physics-world:terrainterrain
setf
physics-world-kernelsworld
if
eqkernels:fastest
kernels
world
defunphysics-world-body-count
world
+
physics-body-columns-length
physics-world-awakeworld
physics-body-columns-length
physics-world-sleepingworld

--------------------------------------------------------------------- Creating, finding, and destroying bodies.

defunspawn-physics-body
worldxyz&key
radius0.25
mass1.0
vx0.0
vy0.0
vz0.0
restitution0.3
friction0.5
rolling-resistance0.01
damping0.05
kind0
collides-with-bodies-pt
hit-report-pnil
never-sleep-pnil
lifetimenil

Add a sphere to world and return its handle.

let*
awake
physics-world-awakeworld
radius
coerceradius'single-float
mass
coercemass'single-float
inverse-mass
if
pluspmass
/mass
0f0
inverse-inertia
if
pluspmass
/
*0.4f0massradiusradius
0f0
local
physics-body-columns-lengthawake
handle
physics-body-columns-pushawakehandle
coercex'single-float
coercey'single-float
coercez'single-float
0f00f00f0
coercevx'single-float
coercevy'single-float
coercevz'single-float
0f00f00f00f00f00f01f0radiusinverse-massinverse-inertia
coercerestitution'single-float
coercefriction'single-float
coercerolling-resistance'single-float
coercedamping'single-float
kind
logior
ifcollides-with-bodies-p+physics-body-collides-with-bodies+0
iflifetime
coercelifetime'single-float
-1f0
0f0
handle
defunphysics-body-alive-p
worldhandle

Whether handle still names a body in world.

and
typephandle'fixnum
physics-id-table-handle-live-p
physics-world-idsworld
handle
defunphysics-body-set-and-local
worldhandle

Return the set constant and local row of handle's body, unchecked.

declare
fixnumhandle
let
ids
physics-world-idsworld
values
aref
physics-id-table-setids
index
aref
physics-id-table-localids
index
defunphysics-body-columns-for-set
worldset
declare
fixnumset
cond
physics-world-awakeworld
physics-world-sleepingworld
t
error"Set ~D holds no bodies."set
defunphysics-body-position
worldhandle

Return handle's body centre as three single-floats, or NIL when dead.

when
multiple-value-bind
setlocal
let
values
aref
physics-body-columns-x-lanecolumns
local
aref
physics-body-columns-y-lanecolumns
local
aref
physics-body-columns-z-lanecolumns
local
defunphysics-body-velocity
worldhandle

Return handle's body velocity as three single-floats, or NIL when dead.

when
multiple-value-bind
setlocal
let
values
aref
physics-body-columns-vx-lanecolumns
local
aref
physics-body-columns-vy-lanecolumns
local
aref
physics-body-columns-vz-lanecolumns
local
defun
velocityworldhandle

Set handle's velocity from a list of three reals, waking the body.

when
multiple-value-bind
setlocal
let
destructuring-bind
vxvyvz
velocity
setf
aref
physics-body-columns-vx-lanecolumns
local
coercevx'single-float
aref
physics-body-columns-vy-lanecolumns
local
coercevy'single-float
aref
physics-body-columns-vz-lanecolumns
local
coercevz'single-float
velocity
defun%relocate-physics-body
worldhandlefrom-setto-set

Move handle's row from from-set to to-set, patching every forwarding address the move disturbs.

declare
fixnumhandlefrom-setto-set
let*
ids
physics-world-idsworld
local
aref
physics-id-table-localids
index
new-local
setf
aref
physics-id-table-setids
index
to-set
aref
physics-id-table-localids
index
new-local
whenmoved

The row that filled the hole belongs to another body: tell it.

let
moved-handle
aref
physics-body-columns-handle-lanefrom
local
setf
aref
physics-id-table-localids
local
new-local
defunwake-physics-body
worldhandle

Bring handle's body into the awake set if it was asleep; return T if so.

when
multiple-value-bind
setlocal
declare
ignorelocal
when
let
setf
aref
physics-body-columns-sleep-time-lane
physics-world-awakeworld
new-local
0f0
physics-event-columns-push
physics-world-eventsworld
:wokehandle+physics-no-body+nil0f00f00f00f0
t
defunsleep-physics-body
worldhandle

Move handle's body out of the awake set; return T if it was awake. #QKS4GZ

when
multiple-value-bind
setlocal
declare
ignorelocal
when
let*
sleeping
physics-world-sleepingworld

A sleeper is still: its velocity is not merely unread.

setf
aref
physics-body-columns-vx-lanesleeping
new-local
0f0
aref
physics-body-columns-vy-lanesleeping
new-local
0f0
aref
physics-body-columns-vz-lanesleeping
new-local
0f0
aref
physics-body-columns-wx-lanesleeping
new-local
0f0
aref
physics-body-columns-wy-lanesleeping
new-local
0f0
aref
physics-body-columns-wz-lanesleeping
new-local
0f0
aref
physics-body-columns-dx-lanesleeping
new-local
0f0
aref
physics-body-columns-dy-lanesleeping
new-local
0f0
aref
physics-body-columns-dz-lanesleeping
new-local
0f0
physics-event-columns-push
physics-world-eventsworld
:slepthandle+physics-no-body+nil0f00f00f00f0
t
defundestroy-physics-body
worldhandle

Remove handle's body from world; its contacts go at the next prune.

when
multiple-value-bind
setlocal
let*
ids
physics-world-idsworld
whenmoved
let
moved-handle
aref
physics-body-columns-handle-lanecolumns
local
setf
aref
physics-id-table-localids
local
t
defunclear-physics-bodies
world

Remove every body and contact from world.

physics-body-columns-reset
physics-world-awakeworld
physics-body-columns-reset
physics-world-sleepingworld
physics-contact-columns-reset
physics-world-contactsworld
clrhash
physics-world-contact-indexworld
let
ids
physics-world-idsworld
dotimes
index
physics-id-table-nextids
unless
=
aref
physics-id-table-setids
index
+physics-set-free+
world
defmacrodo-physics-bodies
handleworld&key
sets'
:awake:sleeping
&bodybody

Run body with handle bound to each body's handle in the named SETS. body must not create or destroy bodies.

let
columns
gensym"COLUMNS"
index
gensym"INDEX"
`
dolist
,columns
list,@
loopforsetinsetscollect
ecaseset
:awake`
physics-world-awake,world
:sleeping`
physics-world-sleeping,world
dotimes
,index
physics-body-columns-length,columns
let
,handle
aref
physics-body-columns-handle-lane,columns
,index
,@body

--------------------------------------------------------------------- Kinematic boxes: posted by the client before each step.

defunclear-physics-boxes
world
physics-box-columns-reset
physics-world-boxesworld
world
defunpost-physics-box
worldmin-xmin-ymin-zmax-xmax-ymax-z&key
vx0.0
vy0.0
vz0.0
owner

Tell world about a moving box that bodies must not enter this step.

physics-box-columns-push
physics-world-boxesworld
coercemin-x'single-float
coercemin-y'single-float
coercemin-z'single-float
coercemax-x'single-float
coercemax-y'single-float
coercemax-z'single-float
coercevx'single-float
coercevy'single-float
coercevz'single-float
owner
world

--------------------------------------------------------------------- The terrain probe: the voxel lattice asked one cell at a time, with the last chunk's storage kept borrowed, so a body's neighbourhood -- almost always inside one chunk -- costs one hash lookup, not twenty-seven. Missing terrain is a wall, as it is for the walking bodies (see world-terrain-solid-p): a body near ground that has not streamed in leans on the boundary rather than falling through the world.

defconstant+terrain-probe-ways+8

How many chunks the probe keeps borrowed at once. A body's neighbourhood straddles at most eight chunks.

defstruct
terrain-probe
:constructormake-terrain-probe
worldnil
width16:typefixnum
height16:typefixnum
depth16:typefixnum
world-height16:typefixnum

The way the last question landed in, checked first.

current0:typefixnum

The next way to evict.

next0:typefixnum

Per way: the chunk coordinate held, its borrowed indices (or NIL for a chunk that is not resident), which palette entries are solid, and how long the palette was when that was computed.

chunk-xs
make-array+terrain-probe-ways+:element-type'fixnum:initial-elementmost-positive-fixnum
:type
simple-arrayfixnum
chunk-ys
make-array+terrain-probe-ways+:element-type'fixnum:initial-elementmost-positive-fixnum
:type
simple-arrayfixnum
chunk-zs
make-array+terrain-probe-ways+:element-type'fixnum:initial-elementmost-positive-fixnum
:type
simple-arrayfixnum
indices
make-array+terrain-probe-ways+:initial-elementnil
:typesimple-vector
solids
let
solids
dotimes
setf
arefsolidsway
make-array16:element-type'bit:initial-element0
:typesimple-vector
palettes
make-array+terrain-probe-ways+:initial-elementnil
:typesimple-vector
palette-lengths
make-array+terrain-probe-ways+:element-type'fixnum:initial-element0
:type
simple-arrayfixnum
defunreset-terrain-probe
probeworld

Point probe at world and forget whatever chunks it was holding.

setf
terrain-probe-worldprobe
world
terrain-probe-currentprobe
0
terrain-probe-nextprobe
0
fill
terrain-probe-chunk-xsprobe
most-positive-fixnum
fill
terrain-probe-indicesprobe
nil
fill
terrain-probe-palettesprobe
nil
whenworld
let
shape
voxel-space-chunk-shape
block-world-spaceworld
setf
terrain-probe-widthprobe
chunk-shape-widthshape
terrain-probe-heightprobe
chunk-shape-heightshape
terrain-probe-depthprobe
chunk-shape-depthshape
terrain-probe-world-heightprobe
chunk-shape-heightshape
probe
defunterrain-probe-refresh-solid
probeway

Recompute the palette solidity bits of the chunk in way.

declare
fixnumway
let*
palette
aref
terrain-probe-palettesprobe
way
bits
aref
terrain-probe-solidsprobe
way
declare
simple-bit-vectorbits
when
<
lengthbits
count
setfbits
make-array
maxcount
*2
lengthbits
:element-type'bit:initial-element0
aref
terrain-probe-solidsprobe
way
bits
dotimes
indexcount
setf
sbitbitsindex
setf
aref
terrain-probe-palette-lengthsprobe
way
count
defunterrain-probe-visit-chunk
probechunk-xchunk-ychunk-z

Borrow chunk chunk-x,Y,Z into a way of probe and return the way.

declare
fixnumchunk-xchunk-ychunk-z
let
way
terrain-probe-nextprobe
world
terrain-probe-worldprobe
setf
terrain-probe-nextprobe
aref
terrain-probe-chunk-xsprobe
way
chunk-x
aref
terrain-probe-chunk-ysprobe
way
chunk-y
aref
terrain-probe-chunk-zsprobe
way
chunk-z
multiple-value-bind
chunkpresent-p
andworld
world-chunk-atworldchunk-xchunk-ychunk-z
cond
present-p
with-block-content-storage
domainpaletteindices
chunk
declare
ignoredomain
setf
aref
terrain-probe-indicesprobe
way
indices
aref
terrain-probe-palettesprobe
way
palette
t
setf
aref
terrain-probe-indicesprobe
way
nil
aref
terrain-probe-palettesprobe
way
nil
way
declaim
defunterrain-probe-way
probechunk-xchunk-ychunk-z

The way holding chunk chunk-x,Y,Z, borrowing it if no way does.

declare
fixnumchunk-xchunk-ychunk-z
optimize
speed3
safety0
let
current
terrain-probe-currentprobe
xs
terrain-probe-chunk-xsprobe
ys
terrain-probe-chunk-ysprobe
zs
terrain-probe-chunk-zsprobe
declare
fixnumcurrent
if
and
=chunk-x
arefxscurrent
=chunk-y
arefyscurrent
=chunk-z
arefzscurrent
current
let
way
or
dotimes
when
and
=chunk-x
arefxsway
=chunk-y
arefysway
=chunk-z
arefzsway
terrain-probe-visit-chunkprobechunk-xchunk-ychunk-z
declare
fixnumway
setf
terrain-probe-currentprobe
way
way
defunterrain-probe-solid-p
probexyz

Whether the cell at world X,Y,Z is solid, or a boundary standing in.

declare
fixnumxyz
optimize
speed3
safety0
let
width
terrain-probe-widthprobe
height
terrain-probe-heightprobe
depth
terrain-probe-depthprobe
declare
fixnumwidthheightdepth
multiple-value-bind
chunk-xlocal-x
multiple-value-bind
chunk-ylocal-y
multiple-value-bind
chunk-zlocal-z
floorzdepth
declare
fixnumchunk-xchunk-ychunk-zlocal-xlocal-ylocal-z
let*
way
terrain-probe-wayprobechunk-xchunk-ychunk-z
indices
aref
terrain-probe-indicesprobe
way
ifindices
let
palette-index
aref
the
simple-array
unsigned-byte16
indices
+local-x
thefixnum
*width
thefixnum
+local-y
thefixnum
*heightlocal-z

A palette can grow under us between visits.

when
>=palette-index
aref
terrain-probe-palette-lengthsprobe
way
=1
sbit
thesimple-bit-vector
aref
terrain-probe-solidsprobe
way
palette-index

Absent: solid below the world's height, air above it.

<y
terrain-probe-world-heightprobe

--------------------------------------------------------------------- The hash grid: every body, awake or asleep, dropped into a cell of a uniform grid keyed by its centre, as a linked list threaded through a NEXT column. Rebuilt from scratch every step; a query walks the twenty-seven cells around a body. Entries are handles.

defstruct
physics-grid
:constructormake-physics-grid
cell-size1f0:typesingle-float
heads
make-array4096:element-type'fixnum:initial-element-1
:type
simple-arrayfixnum
mask4095:typefixnum
nexts
make-array256:element-type'fixnum:initial-element-1
:type
simple-arrayfixnum
handles
make-array256:element-type'fixnum:initial-element0
:type
simple-arrayfixnum
count0:typefixnum
declaim
defunphysics-grid-cell
gridxyz
declare
single-floatxyz
optimize
speed3
safety0
let*
inverse
/
physics-grid-cell-sizegrid
ix
thefixnum
floor
*xinverse
iy
thefixnum
floor
*yinverse
iz
thefixnum
floor
*zinverse
declare
fixnumixiyiz

A constant mask keeps the multiplies in modular machine arithmetic.

logand
logand
logxor
*ix73856093
*iy19349663
*iz83492791
#x3fffffff
physics-grid-maskgrid
defunrebuild-physics-grid
world

Drop every body of world into the grid, sized to the largest body.

let*
grid
physics-world-gridworld
awake
physics-world-awakeworld
sleeping
physics-world-sleepingworld
total
+
physics-body-columns-lengthawake
physics-body-columns-lengthsleeping

Enough cells that the lists stay short, as a power of two.

let
wanted
max4096
let
n1
loopwhile
<n
*2total
do
setfn
*n2
n
unless
=wanted
length
physics-grid-headsgrid
setf
physics-grid-headsgrid
make-arraywanted:element-type'fixnum:initial-element-1
physics-grid-maskgrid
1-wanted
when
<
length
physics-grid-nextsgrid
total
let
capacity
maxtotal
*2
length
physics-grid-nextsgrid
setf
physics-grid-nextsgrid
make-arraycapacity:element-type'fixnum:initial-element-1
physics-grid-handlesgrid
make-arraycapacity:element-type'fixnum:initial-element0
fill
physics-grid-headsgrid
-1
let
largest0.05f0
declare
single-floatlargest
dolist
columns
listawakesleeping
records:with-columnar-buffer-storage
lengthrow
radiiradius
columnsphysics-body-columns
declare
ignorerow
dotimes
indexlength
setflargest
maxlargest
arefradiiindex

A cell holds the largest body with room to spare, so a query needs only the neighbouring cells.

setf
physics-grid-cell-sizegrid
*2.5f0largest
setf
physics-grid-countgrid
0
let
heads
physics-grid-headsgrid
nexts
physics-grid-nextsgrid
handles
physics-grid-handlesgrid
slot0
declare
fixnumslot
dolist
columns
listawakesleeping
records:with-columnar-buffer-storage
lengthrow
xsx
ysy
zsz
hshandle
columnsphysics-body-columns
declare
ignorerow
dotimes
indexlength
let
cell
physics-grid-cellgrid
arefxsindex
arefysindex
arefzsindex
setf
arefhandlesslot
arefhsindex
arefnextsslot
arefheadscell
arefheadscell
slot
incfslot
setf
physics-grid-countgrid
slot
grid
defmacrodo-physics-grid-neighbours
handlegridxyz
&bodybody

Run body with handle bound to each body in the grid cells around X,Y,Z.

let
g
gensym"GRID"
cell-size
gensym"CELL"
cx
gensym
cy
gensym
cz
gensym
dx
gensym
dy
gensym
dz
gensym
slot
gensym"SLOT"
`
let*
,g,grid
,cell-size
physics-grid-cell-size,g
,cx
-,x,cell-size
,cy
-,y,cell-size
,cz
-,z,cell-size
declare
single-float,cell-size,cx,cy,cz
dotimes
,dx3
dotimes
,dy3
dotimes
,dz3
let
,slot
aref
physics-grid-heads,g
physics-grid-cell,g
+,cx
*,dx,cell-size
+,cy
*,dy,cell-size
+,cz
*,dz,cell-size
declare
fixnum,slot
loopuntil
minusp,slot
do
let
,handle
aref
physics-grid-handles,g
,slot
declare
fixnum,handle
,@body
setf,slot
aref
physics-grid-nexts,g
,slot

--------------------------------------------------------------------- Finding contacts. The key packs the pair; the hash gives the row.

declaim
defunphysics-contact-key
kindindex-aother

Pack a contact key: kind in the low byte, other above it, index-a on top.

declare
fixnumkindindex-aother
logior
thefixnum
ashindex-a32
thefixnum
ash
logandother#xffffff
8
kind
declaim
defunphysics-cell-key
xyz
declare
fixnumxyz
logior
ash
logandx#xff
16
ash
logandy#xff
8
logandz#xff
defunensure-physics-contact
worldkeykindhandle-ahandle-bowner

Return the row of the contact key, making it if the pair is new. The row's LAST-STEP is stamped with the current step.

declare
fixnumkeykindhandle-ahandle-b
optimize
speed3
safety1
let*
contacts
physics-world-contactsworld
index
physics-world-contact-indexworld
step
physics-world-step-countworld
row
gethashkeyindex
declare
fixnumstep
cond
androw
=handle-a
arefrow
=handle-b
arefrow
setf
arefrow
step
arefrow
owner
row
row

The key survived its pair: a body index came back to life. Keep the row, forget what it carried.

setf
arefrow
handle-a
arefrow
handle-b
arefrow
owner
arefrow
step
arefrow
0
aref
%lanephysics-contact-columnscontactsnormal-impulse
row
0f0
aref
%lanephysics-contact-columnscontactstangent-impulse-1
row
0f0
aref
%lanephysics-contact-columnscontactstangent-impulse-2
row
0f0
aref
%lanephysics-contact-columnscontactsrolling-impulse-x
row
0f0
aref
%lanephysics-contact-columnscontactsrolling-impulse-y
row
0f0
aref
%lanephysics-contact-columnscontactsrolling-impulse-z
row
0f0
row
t
let
new
physics-contact-columns-lengthcontacts
physics-contact-columns-pushcontactskeykindhandle-ahandle-bownerstep00f00f00f00f00f00f00f00f00f00f00f00f00f00f00f00f0
setf
gethashkeyindex
new
incf
physics-world-contacts-madeworld
new
defunprune-physics-contacts
world

Drop every contact whose pair was not within reach this step, ending those that were touching.

let*
contacts
physics-world-contactsworld
index
physics-world-contact-indexworld
events
physics-world-eventsworld
step
physics-world-step-countworld
row
1-
physics-contact-columns-lengthcontacts
declare
fixnumrow
loopwhile
>=row0
do
let
last
aref
physics-contact-columns-last-step-lanecontacts
row
when
<laststep
when
plusp
aref
physics-contact-columns-touching-lanecontacts
row
physics-event-columns-pushevents:end
aref
physics-contact-columns-handle-a-lanecontacts
row
aref
physics-contact-columns-handle-b-lanecontacts
row
aref
physics-contact-columns-owner-lanecontacts
row
0f00f00f00f0
remhash
aref
physics-contact-columns-key-lanecontacts
row
index
incf
physics-world-contacts-droppedworld
let
whenmoved
setf
gethash
aref
physics-contact-columns-key-lanecontacts
row
index
row
decfrow
world

--------------------------------------------------------------------- Contact generation. Each generator writes one manifold into a contact row: normal from A toward the other side, the point on the other side, the other side's velocity, and the separation.

defun%record-physics-manifold
worldrownxnynzpxpypzkvxkvykvzseparation
declare
fixnumrow
single-floatnxnynzpxpypzkvxkvykvzseparation
optimize
speed3
safety0
let
contacts
physics-world-contactsworld
setfnxnynzpxpypzkvxkvykvz
arefrow
separation
row
defungenerate-physics-body-pairs
world

Find every awake body's neighbours in the grid and give each near pair a contact. A moving awake body wakes a sleeping neighbour first; a slow one leans on it as on terrain. Woken bodies join the awake set at its end and are visited by this same pass.

declare
optimize
speed3
safety1
let*
awake
physics-world-awakeworld
sleeping
physics-world-sleepingworld
grid
physics-world-gridworld
ids
physics-world-idsworld
reach
wake-speed
coerce*physics-wake-speed*'single-float
i0
declare
fixnumi
single-floatreachwake-speed
macrolet
awake-lane
name
`
%lanephysics-body-columnsawake,name
other-lane
name

The other body's set, chosen per neighbour.

`
ifawake-b-p
%lanephysics-body-columnsawake,name
%lanephysics-body-columnssleeping,name

A wake appends to the awake set and may reallocate its lanes, so every lane is fetched afresh per body rather than bound once.

loopwhile
<i
physics-body-columns-lengthawake
do
when
logtest
aref
awake-laneflags
i
+physics-body-collides-with-bodies+
let*
handle-a
aref
awake-lanehandle
i
xa
aref
awake-lanex
i
ya
aref
awake-laney
i
za
aref
awake-lanez
i
ra
aref
awake-laneradius
i
vx
aref
awake-lanevx
i
vy
aref
awake-lanevy
i
vz
aref
awake-lanevz
i
moving-p
>
+
*vxvx
*vyvy
*vzvz
*wake-speedwake-speed
index-a
declare
single-floatxayazaravxvyvz
fixnumhandle-aindex-a
do-physics-grid-neighbours
handle-bgridxayaza
unless
=handle-bhandle-a
let*
index-b
set-b
aref
physics-id-table-setids
index-b
local-b
aref
physics-id-table-localids
index-b
declare
fixnumindex-bset-blocal-b
when

Near enough to wake? Check before relocating.

let*
dx
-
aref
%lanephysics-body-columnssleepingx
local-b
xa
dy
-
aref
%lanephysics-body-columnssleepingy
local-b
ya
dz
-
aref
%lanephysics-body-columnssleepingz
local-b
za
limit
+ra
aref
%lanephysics-body-columnssleepingradius
local-b
reach
declare
single-floatdxdydzlimit
when
<
+
*dxdx
*dydy
*dzdz
*limitlimit
wake-physics-bodyworldhandle-b

Waking only appends to the awake set, so our own row I is where it was; B's is not.

setfset-b
aref
physics-id-table-setids
index-b
local-b
aref
physics-id-table-localids
index-b
let
awake-b-p
when
and
logtest
aref
other-laneflags
local-b
+physics-body-collides-with-bodies+

Each awake pair once: from the lower row.

or
notawake-b-p
<ilocal-b
let*
xb
aref
other-lanex
local-b
yb
aref
other-laney
local-b
zb
aref
other-lanez
local-b
rb
aref
other-laneradius
local-b
dx
-xbxa
dy
-ybya
dz
-zbza
limit
+rarbreach
distance-squared
+
*dxdx
*dydy
*dzdz
declare
single-floatxbybzbrbdxdydzlimitdistance-squared
when
<distance-squared
*limitlimit
let*
distance
sqrt
the
single-float0f0
distance-squared
nx
ny
nz
low
minindex-aindex-b
high
maxindex-aindex-b

A is the awake body of the pair, and when both are awake, the lower row.

row
declare
single-floatdistancenxnynz
fixnumrow
%record-physics-manifoldworldrownxnynz
-xb
*rbnx
-yb
*rbny
-zb
*rbnz
0f00f00f0
incfi
world
defmacro%with-sphere-box-contact
nxnynzpxpypzseparation
cxcyczradiusmin-xmin-ymin-zmax-xmax-ymax-zreach&key
accept-pt
&bodybody

Run body when the sphere at cx,CY,CZ is within REACH of the box, binding its contact normal, point, and separation. ACCEPT-P is evaluated with OUTSIDE-X OUTSIDE-Y OUTSIDE-Z bound to -1, 0, or 1 for the sphere centre's position against each axis of the box, and may refuse the contact.

let
qx
gensym
qy
gensym
qz
gensym
dx
gensym
dy
gensym
dz
gensym
distance-squared
gensym
distance
gensym
`
let*
,qx
max,min-x
min,max-x,cx
,qy
max,min-y
min,max-y,cy
,qz
max,min-z
min,max-z,cz
,dx
-,cx,qx
,dy
-,cy,qy
,dz
-,cz,qz
,distance-squared
+
*,dx,dx
*,dy,dy
*,dz,dz
declare
single-float,qx,qy,qz,dx,dy,dz,distance-squared
let
outside-x
cond
<,cx,min-x
-1
>,cx,max-x
1
t0
outside-y
cond
<,cy,min-y
-1
>,cy,max-y
1
t0
outside-z
cond
<,cz,min-z
-1
>,cz,max-z
1
t0
declare
fixnumoutside-xoutside-youtside-z
ignorableoutside-xoutside-youtside-z
cond
plusp,distance-squared

Outside the box: the closest point is on a face, edge, or corner.

when
and
<,distance-squared
*,reach,reach
,accept-p

The normal points from the sphere toward the box, as every

contact normal points from A toward its other side.

let*
,distance
sqrt
the
single-float0f0
,distance-squared
,nx
,ny
,nz
,px,qx
,py,qy
,pz,qz
,separation
-,distance,radius
declare
single-float,distance,nx,ny,nz,px,py,pz,separation
,@body
t

The centre is inside the box: push out through the nearest

face. ACCEPT-P then sees the face's direction.

let*
face-x
if
<
-,cx,min-x
-,max-x,cx
-11
face-y
if
<
-,cy,min-y
-,max-y,cy
-11
face-z
if
<
-,cz,min-z
-,max-z,cz
-11
depth-x
if
minuspface-x
-,cx,min-x
-,max-x,cx
depth-y
if
minuspface-y
-,cy,min-y
-,max-y,cy
depth-z
if
minuspface-z
-,cz,min-z
-,max-z,cz
declare
fixnumface-xface-yface-z
single-floatdepth-xdepth-ydepth-z
multiple-value-bind
outside-xoutside-youtside-z,px,py,pz,separation
cond
and
<=depth-ydepth-x
<=depth-ydepth-z
values0face-y0,cx
if
minuspface-y
,min-y,max-y
,cz
-
-depth-y
,radius
<=depth-xdepth-z
valuesface-x00
if
minuspface-x
,min-x,max-x
,cy,cz
-
-depth-x
,radius
t
values00face-z,cx,cy
if
minuspface-z
,min-z,max-z
-
-depth-z
,radius
declare
fixnumoutside-xoutside-youtside-z
single-float,px,py,pz,separation
ignorableoutside-xoutside-youtside-z

Into the box: against the face's outward direction.

let
,nx
-
floatoutside-x0f0
,ny
-
floatoutside-y0f0
,nz
-
floatoutside-z0f0
declare
single-float,nx,ny,nz
when,accept-p,@body
defungenerate-physics-terrain-contacts
world

Give every awake body a contact with each exposed terrain face it is near.

The voxel grid is the static tree (#D9W4CH). A cell's face, edge, or corner is only offered when no solid cell lies in the direction the sphere's centre overshoots it, which is what keeps a ball rolling across a floor of cells from catching on the seams between them: the neighbouring cell's own face is always the nearer, truer contact, and it is the only one made. #MYWH16

let*
awake
physics-world-awakeworld
probe
physics-world-probeworld
margin
coerce*physics-terrain-margin*'single-float
declare
single-floatmargin
unless
physics-world-terrainworld
records:with-columnar-buffer-storage
lengthrow
xsx
ysy
zsz
radiiradius
handleshandle
awakephysics-body-columns
declare
ignorerow
dotimes
let*
cx
arefxsi
cy
arefysi
cz
arefzsi
radius
arefradiii
handle
arefhandlesi

The neighbour rule needs the centre within one cell of the cells it looks at.

reach
min0.98f0
+radiusmargin
min-x
floor
-cxreach
max-x
floor
+cxreach
min-y
floor
-cyreach
max-y
floor
+cyreach
min-z
floor
-czreach
max-z
floor
+czreach
declare
single-floatcxcyczradiusreach
fixnummin-xmax-xmin-ymax-ymin-zmax-zindex-a
loopforxfixnumfrommin-xtomax-xdo
loopforyfixnumfrommin-ytomax-ydo
loopforzfixnumfrommin-ztomax-zdo
when
let
box-min-x
floatx0f0
box-min-y
floaty0f0
box-min-z
floatz0f0
declare
single-floatbox-min-xbox-min-ybox-min-z
%with-sphere-box-contact
nxnynzpxpypzseparation
cxcyczradiusbox-min-xbox-min-ybox-min-z
+box-min-x1f0
+box-min-y1f0
+box-min-z1f0
reach:accept-p

No solid neighbour in any overshoot direction.

not
or
and
/=outside-x0
terrain-probe-solid-pprobe
+xoutside-x
yz
and
/=outside-y0
terrain-probe-solid-pprobex
+youtside-y
z
and
/=outside-z0
terrain-probe-solid-pprobexy
+zoutside-z
and
/=outside-x0
/=outside-y0
terrain-probe-solid-pprobe
+xoutside-x
+youtside-y
z
and
/=outside-x0
/=outside-z0
terrain-probe-solid-pprobe
+xoutside-x
y
+zoutside-z
and
/=outside-y0
/=outside-z0
terrain-probe-solid-pprobex
+youtside-y
+zoutside-z
and
/=outside-x0
/=outside-y0
/=outside-z0
terrain-probe-solid-pprobe
+xoutside-x
+youtside-y
+zoutside-z
world
defungenerate-physics-box-contacts
world

Give every awake body a contact with each kinematic box it is near.

let*
awake
physics-world-awakeworld
boxes
physics-world-boxesworld
margin
coerce*physics-terrain-margin*'single-float
declare
single-floatmargin
records:with-columnar-buffer-storage
box-countbox-row
box-min-xsmin-x
box-min-ysmin-y
box-min-zsmin-z
box-max-xsmax-x
box-max-ysmax-y
box-max-zsmax-z
box-vxsvx
box-vysvy
box-vzsvz
boxesphysics-box-columns
declare
ignorebox-row
when
zeropbox-count

A moving box wakes the sleepers it reaches, before the awake pass so that they are in it.

let
wokennil
records:with-columnar-buffer-storage
countrow
xsx
ysy
zsz
radiiradius
handleshandle
physics-world-sleepingworld
physics-body-columns
declare
ignorerow
dotimes
icount
let
cx
arefxsi
cy
arefysi
cz
arefzsi
reach
+
arefradiii
margin
dotimes
bbox-count
when
and
or
/=0f0
arefbox-vxsb
/=0f0
arefbox-vysb
/=0f0
arefbox-vzsb
<
-cxreach
arefbox-max-xsb
>
+cxreach
arefbox-min-xsb
<
-cyreach
arefbox-max-ysb
>
+cyreach
arefbox-min-ysb
<
-czreach
arefbox-max-zsb
>
+czreach
arefbox-min-zsb
push
arefhandlesi
woken
dolist
handlewoken
records:with-columnar-buffer-storage
lengthrow
xsx
ysy
zsz
radiiradius
handleshandle
awakephysics-body-columns
declare
ignorerow
dotimes
let*
cx
arefxsi
cy
arefysi
cz
arefzsi
radius
arefradiii
handle
arefhandlesi
reach
+radiusmargin
declare
single-floatcxcyczradiusreach
fixnumindex-a
dotimes
bbox-count
let
min-x
arefbox-min-xsb
min-y
arefbox-min-ysb
min-z
arefbox-min-zsb
max-x
arefbox-max-xsb
max-y
arefbox-max-ysb
max-z
arefbox-max-zsb
when
and
<
-cxreach
max-x
>
+cxreach
min-x
<
-cyreach
max-y
>
+cyreach
min-y
<
-czreach
max-z
>
+czreach
min-z
%with-sphere-box-contact
nxnynzpxpypzseparation
cxcyczradiusmin-xmin-ymin-zmax-xmax-ymax-zreach
let
%record-physics-manifoldworldrownxnynzpxpypz
arefbox-vxsb
arefbox-vysb
arefbox-vzsb
separation
world

--------------------------------------------------------------------- Softness (#R7F2QH): three numbers from a frequency, a damping ratio, and the substep.

declaim
defunphysics-softness
hertzzetah

Return BIAS-RATE, MASS-SCALE, and IMPULSE-SCALE for a soft constraint.

declare
single-floathertzzetah
if
zerophertz
values0f01f00f0
let*
omega
*2f0
floatpi0f0
hertz
a1
+
*2f0zeta
*homega
a2
*homegaa1
a3
/
+1f0a2
values
/omegaa1
*a2a3
a3

--------------------------------------------------------------------- Colouring and preparation. Every live contact is given a colour such that no two contacts in one colour share an awake body, then the constraint buffer is filled in colour order with all the solver needs.

defunensure-physics-color-scratch
worldawake-countcontact-count
let
bits
physics-world-color-bitsworld
colors
physics-world-contact-colorsworld
dotimes
color
lengthbits
let
vector
arefbitscolor
if
andvector
>=
lengthvector
awake-count
fillvector0:endawake-count
setf
arefbitscolor
make-array
max64
*2awake-count
:element-type'bit:initial-element0
when
<
lengthcolors
contact-count
setf
physics-world-contact-colorsworld
make-array
max64
*2contact-count
:element-type'fixnum:initial-element0
world
defun%ensure-physics-constraint-capacity
buffercount
when
<
physics-constraint-columns-capacitybuffer
count
%physics-constraint-columns-growbuffercount
setf
physics-constraint-columns-lengthbuffer
count
buffer
defunprepare-physics-constraints
worldh

Colour the live contacts and fill the constraint buffer in colour order. Return the number of colours in use, the overflow colour counted. #SSMPYW

declare
single-floath
optimize
speed3
safety1
let*
awake
physics-world-awakeworld
contacts
physics-world-contactsworld
constraints
physics-world-constraintsworld
ids
physics-world-idsworld
awake-count
physics-body-columns-lengthawake
contact-count
physics-contact-columns-lengthcontacts
dynamic-colors
-max-colors4
overflowmax-colors
starts
physics-world-color-startsworld
declare
fixnumawake-countcontact-countmax-colorsdynamic-colorsoverflow
ensure-physics-color-scratchworldawake-countcontact-count
let
bits
physics-world-color-bitsworld
colors
physics-world-contact-colorsworld
handle-as
physics-contact-columns-handle-a-lanecontacts
handle-bs
physics-contact-columns-handle-b-lanecontacts
kinds
physics-contact-columns-kind-lanecontacts
declare
type
simple-arrayfixnum
colorshandle-ashandle-bskinds
fillstarts0

Pass one: colour.

dotimes
rowcontact-count
let*
handle-a
arefhandle-asrow
local-a
aref
physics-id-table-localids
dynamic-b-p
and
=
aref
physics-id-table-setids
physics-handle-index
arefhandle-bsrow
+physics-set-awake+
local-b
ifdynamic-b-p
aref
physics-id-table-localids
physics-handle-index
arefhandle-bsrow
-1
coloroverflow
declare
fixnumlocal-alocal-bcolor
ifdynamic-b-p
loopforcfixnumfrom0belowdynamic-colorsdo
let
vector
arefbitsc
declare
simple-bit-vectorvector
when
and
zerop
sbitvectorlocal-a
zerop
sbitvectorlocal-b
setf
sbitvectorlocal-a
1
sbitvectorlocal-b
1
colorc

Static contacts colour from the top down: solved last.

loopforcfixnumfrom
1-max-colors
downto0do
let
vector
arefbitsc
declare
simple-bit-vectorvector
when
zerop
sbitvectorlocal-a
setf
sbitvectorlocal-a
1
colorc
setf
arefcolorsrow
color
incf
arefstarts
1+color

Prefix sums: STARTS[c] is where colour C begins.

loopforcfrom1to
1+overflow
do
incf
arefstartsc
arefstarts
1-c
%ensure-physics-constraint-capacityconstraintscontact-count

Pass two: fill, with a moving cursor per colour.

let
cursor
make-array
+2max-colors
:element-type'fixnum
declare
dynamic-extentcursor
replacecursorstarts
multiple-value-bind
dynamic-biasdynamic-mass-scaledynamic-impulse-scale
multiple-value-bind
static-biasstatic-mass-scalestatic-impulse-scale
physics-softness
min
*2f0
coerce*physics-contact-hertz*'single-float
*0.125f0
/h
*0.5f0
h
let
slop
coerce*physics-linear-slop*'single-float
dummyawake-count
declare
single-floatslop
fixnumdummy
dotimes
rowcontact-count
let*
color
arefcolorsrow
slot
arefcursorcolor
handle-a
arefhandle-asrow
local-a
aref
physics-id-table-localids
dynamic-b-p
and
=
aref
physics-id-table-setids
physics-handle-index
arefhandle-bsrow
+physics-set-awake+
local-b
ifdynamic-b-p
aref
physics-id-table-localids
physics-handle-index
arefhandle-bsrow
dummy
nx
ny
nz
ra
aref
%lanephysics-body-columnsawakeradius
local-a
rb
ifdynamic-b-p
aref
%lanephysics-body-columnsawakeradius
local-b
0f0
ima
aref
%lanephysics-body-columnsawakeinverse-mass
local-a
imb
aref
%lanephysics-body-columnsawakeinverse-mass
local-b
iia
aref
%lanephysics-body-columnsawakeinverse-inertia
local-a
iib
aref
%lanephysics-body-columnsawakeinverse-inertia
local-b

Anchors: from each centre to the contact point. A's runs along the normal; B's against it.

rax
*ranx
ray
*rany
raz
*ranz
rbx
-
*rbnx
rby
-
*rbny
rbz
-
*rbnz
normal-mass
let
k
+imaimb
if
pluspk
/k
0f0
tangent-mass
let
k
+imaimb
*iiarara
*iibrbrb
if
pluspk
/k
0f0
rolling-mass
let
k
+iiaiib
if
pluspk
/k
0f0

A tangent frame from the normal.

t1x0f0
t1y0f0
t1z0f0
declare
fixnumcolorslotlocal-alocal-b
single-floatnxnynzrarbimaimbiiaiibraxrayrazrbxrbyrbznormal-masstangent-massrolling-masst1xt1yt1z
if
>
absny
0.9f0

The normal is near vertical: cross with X.

let*
cynz
cz
-ny
l
sqrt
the
single-float0f0
+
*cycy
*czcz
setft1x0f0t1y
/cyl
t1z
/czl

Cross with Y.

let*
cx
-nz
cznx
l
sqrt
the
single-float0f0
+
*cxcx
*czcz
setft1x
/cxl
t1y0f0t1z
/czl
let*
t2x
-
*nyt1z
*nzt1y
t2y
-
*nzt1x
*nxt1z
t2z
-
*nxt1y
*nyt1x
kvx
kvy
kvz

Approach speed now, for restitution later.

vax
aref
%lanephysics-body-columnsawakevx
local-a
vay
aref
%lanephysics-body-columnsawakevy
local-a
vaz
aref
%lanephysics-body-columnsawakevz
local-a
vbx
+
aref
%lanephysics-body-columnsawakevx
local-b
kvx
vby
+
aref
%lanephysics-body-columnsawakevy
local-b
kvy
vbz
+
aref
%lanephysics-body-columnsawakevz
local-b
kvz
relative-velocity
+
*
-vbxvax
nx
*
-vbyvay
ny
*
-vbzvaz
nz
restitution
ifdynamic-b-p
max
aref
%lanephysics-body-columnsawakerestitution
local-a
aref
%lanephysics-body-columnsawakerestitution
local-b
aref
%lanephysics-body-columnsawakerestitution
local-a
friction
ifdynamic-b-p
sqrt
the
single-float0f0
*
aref
%lanephysics-body-columnsawakefriction
local-a
aref
%lanephysics-body-columnsawakefriction
local-b
aref
%lanephysics-body-columnsawakefriction
local-a
rolling
ifdynamic-b-p
max
aref
%lanephysics-body-columnsawakerolling-resistance
local-a
aref
%lanephysics-body-columnsawakerolling-resistance
local-b
aref
%lanephysics-body-columnsawakerolling-resistance
local-a
declare
single-floatt2xt2yt2zkvxkvykvzvaxvayvazvbxvbyvbzrelative-velocityrestitutionfrictionrolling
macrolet
put
lanevalue
`
setf
arefslot
,value
putcontactrow
putbody-alocal-a
putbody-blocal-b
putnxnx
putnyny
putnznz
putt1xt1x
putt1yt1y
putt1zt1z
putt2xt2x
putt2yt2y
putt2zt2z
putraxrax
putrayray
putrazraz
putrbxrbx
putrbyrby
putrbzrbz
putkvxkvx
putkvykvy
putkvzkvz
putseparation
+
arefrow
slop
putnormal-massnormal-mass
puttangent-masstangent-mass
putrolling-massrolling-mass
putrestitutionrestitution
putfrictionfriction
putrolling-resistancerolling
putrelative-velocityrelative-velocity
puttotal-normal-impulse0f0
ifdynamic-b-p
progn
putbias-rate
*dynamic-mass-scaledynamic-bias
putmass-scaledynamic-mass-scale
putimpulse-scaledynamic-impulse-scale
progn
putbias-rate
*static-mass-scalestatic-bias
putmass-scalestatic-mass-scale
putimpulse-scalestatic-impulse-scale

Warm start from what the pair carried.

putnormal-impulse
aref
%lanephysics-contact-columnscontactsnormal-impulse
row
puttangent-impulse-1
aref
%lanephysics-contact-columnscontactstangent-impulse-1
row
puttangent-impulse-2
aref
%lanephysics-contact-columnscontactstangent-impulse-2
row
putrolling-impulse-x
aref
%lanephysics-contact-columnscontactsrolling-impulse-x
row
putrolling-impulse-y
aref
%lanephysics-contact-columnscontactsrolling-impulse-y
row
putrolling-impulse-z
aref
%lanephysics-contact-columnscontactsrolling-impulse-z
row
setf
arefcursorcolor
1+slot

How many colours actually got contacts.

loopforcfrom0tooverflowcount
<
arefstartsc
arefstarts
1+c

--------------------------------------------------------------------- Kernels. Each phase of a substep is a generic function on the world's kernel family; the methods here are the scalar reference kernels, one contact at a time over the borrowed columns. PHYSICS-SIMD.LISP adds the four-wide families, which run the same arithmetic in the same order on four contacts of one colour at once and hand the tail back here. A kernel is given a half-open range of constraint rows and must touch no other; that is what the colouring guarantees is safe.

defvar*fastest-physics-kernels*:scalar

The kernel family make-physics-world picks by default; PHYSICS-SIMD.LISP sets it to a native family when one is available.

defgenericphysics-integrate-velocities
kernelsawakeh
:documentation

Apply gravity and damping to the awake bodies over H.

defgenericphysics-integrate-positions
kernelsawakeh
:documentation

Advance the awake bodies' deltas and orientations by H.

defgenericphysics-warm-start
kernelsconstraintsawakestarts
:documentation

Apply the carried impulses of every constraint, colour by colour. starts gives each colour's first row, with one more entry than colours.

defgenericphysics-solve-contacts
kernelsconstraintsawakestartsinv-huse-bias-pelapsedpush-max
:documentation

One Gauss-Seidel iteration over every colour in turn: normal impulses with soft or speculative bias when use-bias-p, and without bias plus friction and rolling resistance when not. elapsed is how far into the step the substep is, for a kinematic other side's motion.

defgenericphysics-apply-restitution
kernelsconstraintsawakestartsthreshold
:documentation

Bounce the constraints that hit hard enough, colour by colour.

defmacrodo-physics-colors
startendstarts
&bodybody

Run body with start and END bound to each non-empty colour's row range.

let
color
gensym"COLOR"
array
gensym"STARTS"
`
let
,array,starts
declare
type
simple-arrayfixnum
,array
loopfor,colorfixnumfrom0below
1-
length,array
do
let
,start
aref,array,color
,end
aref,array
1+,color
declare
fixnum,start,end
when
<,start,end
,@body

The columns every contact kernel borrows, bound by name.

defmacrowith-physics-kernel-columns
constraintsawake
&bodybody
`
records:with-columnar-buffer-storage
constraint-countconstraint-row
body-asbody-a
body-bsbody-b
nxsnx
nysny
nzsnz
t1xst1x
t1yst1y
t1zst1z
t2xst2x
t2yst2y
t2zst2z
raxsrax
raysray
razsraz
rbxsrbx
rbysrby
rbzsrbz
kvxskvx
kvyskvy
kvzskvz
separationsseparation
normal-massesnormal-mass
tangent-massestangent-mass
rolling-massesrolling-mass
restitutionsrestitution
frictionsfriction
rolling-resistancesrolling-resistance
relative-velocitiesrelative-velocity
total-normal-impulsestotal-normal-impulse
bias-ratesbias-rate
mass-scalesmass-scale
impulse-scalesimpulse-scale
normal-impulsesnormal-impulse
tangent-impulses-1tangent-impulse-1
tangent-impulses-2tangent-impulse-2
rolling-impulses-xrolling-impulse-x
rolling-impulses-yrolling-impulse-y
rolling-impulses-zrolling-impulse-z
,constraintsphysics-constraint-columns
declare
ignorableconstraint-countconstraint-rowbody-asbody-bsnxsnysnzst1xst1yst1zst2xst2yst2zsraxsraysrazsrbxsrbysrbzskvxskvyskvzsseparationsnormal-massestangent-massesrolling-massesrestitutionsfrictionsrolling-resistancesrelative-velocitiestotal-normal-impulsesbias-ratesmass-scalesimpulse-scalesnormal-impulsestangent-impulses-1tangent-impulses-2rolling-impulses-xrolling-impulses-yrolling-impulses-z
records:with-columnar-buffer-storage
awake-countawake-row
vxsvx
vysvy
vzsvz
wxswx
wyswy
wzswz
dxsdx
dysdy
dzsdz
inverse-massesinverse-mass
inverse-inertiasinverse-inertia
,awakephysics-body-columns
declare
ignorableawake-countawake-rowvxsvysvzswxswyswzsdxsdysdzsinverse-massesinverse-inertias
,@body
defmethodphysics-integrate-velocities
kernels
eql:scalar
awakeh
declare
single-floath
records:with-columnar-buffer-storage
countrow
vxsvx
vysvy
vzsvz
wxswx
wyswy
wzswz
dampingsdamping
inverse-massesinverse-mass
awakephysics-body-columns
declare
ignorerow
optimize
speed3
safety0
let
gravity
*h
coerce*physics-gravity*'single-float
declare
single-floatgravity
dotimes
icount
let
scale
/
+1f0
*h
arefdampingsi
declare
single-floatscale
setf
arefvxsi
*scale
arefvxsi
arefvysi
+
*scale
arefvysi
if
plusp
arefinverse-massesi
gravity0f0
arefvzsi
*scale
arefvzsi
arefwxsi
*scale
arefwxsi
arefwysi
*scale
arefwysi
arefwzsi
*scale
arefwzsi
awake
defmethodphysics-integrate-positions
kernels
eql:scalar
awakeh
declare
single-floath
records:with-columnar-buffer-storage
countrow
vxsvx
vysvy
vzsvz
dxsdx
dysdy
dzsdz
awakephysics-body-columns
declare
ignorerow
optimize
speed3
safety0
dotimes
icount
setf
arefdxsi
+
arefdxsi
*h
arefvxsi
arefdysi
+
arefdysi
*h
arefvysi
arefdzsi
+
arefdzsi
*h
arefvzsi
awake

The orientation integration the wide position kernel shares with the scalar one: per body, a quaternion step and a normalization.

defun%physics-integrate-orientations
awakeh
declare
single-floath
records:with-columnar-buffer-storage
countrow
wxswx
wyswy
wzswz
qxsqx
qysqy
qzsqz
qwsqw
awakephysics-body-columns
declare
ignorerow
optimize
speed3
safety0
let
half
*0.5f0h
declare
single-floathalf
dotimes
icount
let*
wx
arefwxsi
wy
arefwysi
wz
arefwzsi
qx
arefqxsi
qy
arefqysi
qz
arefqzsi
qw
arefqwsi
nx
+qx
*half
+
*wxqw
*wyqz
-
*wzqy
ny
+qy
*half
+
*wyqw
*wzqx
-
*wxqz
nz
+qz
*half
+
*wzqw
*wxqy
-
*wyqx
nw
+qw
*half
-
+
*wxqx
*wyqy
*wzqz
length
sqrt
the
single-float0f0
+
*nxnx
*nyny
*nznz
*nwnw
inverse
if
>length1e-12
1f0
declare
single-floatwxwywzqxqyqzqwnxnynznwlengthinverse
setf
arefqxsi
*nxinverse
arefqysi
*nyinverse
arefqzsi
*nzinverse
arefqwsi
*nwinverse
awake
defun%physics-warm-start-scalar
constraintsawakestartend
declare
fixnumstartend
with-physics-kernel-columns
constraintsawake
declare
optimize
speed3
safety0
loopforcfixnumfromstartbelowenddo
let*
ia
arefbody-asc
ib
arefbody-bsc
nx
arefnxsc
ny
arefnysc
nz
arefnzsc
lambda-n
arefnormal-impulsesc
f1
areftangent-impulses-1c
f2
areftangent-impulses-2c
px
+
*lambda-nnx
*f1
areft1xsc
*f2
areft2xsc
py
+
*lambda-nny
*f1
areft1ysc
*f2
areft2ysc
pz
+
*lambda-nnz
*f1
areft1zsc
*f2
areft2zsc
rax
arefraxsc
ray
arefraysc
raz
arefrazsc
rbx
arefrbxsc
rby
arefrbysc
rbz
arefrbzsc
ima
arefinverse-massesia
imb
arefinverse-massesib
iia
arefinverse-inertiasia
iib
arefinverse-inertiasib
rx
arefrolling-impulses-xc
ry
arefrolling-impulses-yc
rz
arefrolling-impulses-zc
declare
fixnumiaib
single-floatnxnynzlambda-nf1f2pxpypzraxrayrazrbxrbyrbzimaimbiiaiibrxryrz
setf
arefvxsia
-
arefvxsia
*imapx
arefvysia
-
arefvysia
*imapy
arefvzsia
-
arefvzsia
*imapz
arefvxsib
+
arefvxsib
*imbpx
arefvysib
+
arefvysib
*imbpy
arefvzsib
+
arefvzsib
*imbpz

Angular: rA x P and rB x P, plus the rolling impulse.

setf
arefwxsia
-
arefwxsia
*iia
+
-
*raypz
*razpy
rx
arefwysia
-
arefwysia
*iia
+
-
*razpx
*raxpz
ry
arefwzsia
-
arefwzsia
*iia
+
-
*raxpy
*raypx
rz
arefwxsib
+
arefwxsib
*iib
+
-
*rbypz
*rbzpy
rx
arefwysib
+
arefwysib
*iib
+
-
*rbzpx
*rbxpz
ry
arefwzsib
+
arefwzsib
*iib
+
-
*rbxpy
*rbypx
rz
constraints
defmethodphysics-warm-start
kernels
eql:scalar
constraintsawakestarts
do-physics-colors
startendstarts
%physics-warm-start-scalarconstraintsawakestartend
constraints
defun%physics-solve-contacts-scalar
constraintsawakestartendinv-huse-bias-pelapsedpush-max
declare
fixnumstartend
single-floatinv-helapsedpush-max
with-physics-kernel-columns
constraintsawake
declare
optimize
speed3
safety0
loopforcfixnumfromstartbelowenddo
let*
ia
arefbody-asc
ib
arefbody-bsc
nx
arefnxsc
ny
arefnysc
nz
arefnzsc
rax
arefraxsc
ray
arefraysc
raz
arefrazsc
rbx
arefrbxsc
rby
arefrbysc
rbz
arefrbzsc
kvx
arefkvxsc
kvy
arefkvysc
kvz
arefkvzsc
ima
arefinverse-massesia
imb
arefinverse-massesib
iia
arefinverse-inertiasia
iib
arefinverse-inertiasib
vax
arefvxsia
vay
arefvysia
vaz
arefvzsia
wax
arefwxsia
way
arefwysia
waz
arefwzsia
vbx
arefvxsib
vby
arefvysib
vbz
arefvzsib
wbx
arefwxsib
wby
arefwysib
wbz
arefwzsib

Current separation from the accumulated deltas.

dpx
+
-
arefdxsib
arefdxsia
*kvxelapsed
dpy
+
-
arefdysib
arefdysia
*kvyelapsed
dpz
+
-
arefdzsib
arefdzsia
*kvzelapsed
s
+
arefseparationsc
*dpxnx
*dpyny
*dpznz
bias0f0
mass-scale1f0
impulse-scale0f0
declare
fixnumiaib
single-floatnxnynzraxrayrazrbxrbyrbzkvxkvykvzimaimbiiaiibvaxvayvazwaxwaywazvbxvbyvbzwbxwbywbzdpxdpydpzsbiasmass-scaleimpulse-scale
cond
>s0f0

Speculative: may close the gap this substep, no more.

setfbias
*sinv-h
use-bias-p
setfbias
max
*
arefbias-ratesc
s
-push-max
mass-scale
arefmass-scalesc
impulse-scale
arefimpulse-scalesc

Relative velocity at the contact point. A's anchor is along the normal, so its spin adds nothing along it, but the general form is kept so the wide kernel can mirror it.

let*
vrax
+vax
-
*wayraz
*wazray
vray
+vay
-
*wazrax
*waxraz
vraz
+vaz
-
*waxray
*wayrax
vrbx
+vbxkvx
-
*wbyrbz
*wbzrby
vrby
+vbykvy
-
*wbzrbx
*wbxrbz
vrbz
+vbzkvz
-
*wbxrby
*wbyrbx
vn
+
*
-vrbxvrax
nx
*
-vrbyvray
ny
*
-vrbzvraz
nz
old
arefnormal-impulsesc
delta
-
-
*
arefnormal-massesc
+
*mass-scalevn
bias
*impulse-scaleold
new
max
+olddelta
0f0
applied
-newold
px
*appliednx
py
*appliedny
pz
*appliednz
declare
single-floatvraxvrayvrazvrbxvrbyvrbzvnolddeltanewappliedpxpypz
setf
arefnormal-impulsesc
new
areftotal-normal-impulsesc
+
areftotal-normal-impulsesc
new
setfvax
-vax
*imapx
vay
-vay
*imapy
vaz
-vaz
*imapz
vbx
+vbx
*imbpx
vby
+vby
*imbpy
vbz
+vbz
*imbpz
setfwax
-wax
*iia
-
*raypz
*razpy
way
-way
*iia
-
*razpx
*raxpz
waz
-waz
*iia
-
*raxpy
*raypx
wbx
+wbx
*iib
-
*rbypz
*rbzpy
wby
+wby
*iib
-
*rbzpx
*rbxpz
wbz
+wbz
*iib
-
*rbxpy
*rbypx
unlessuse-bias-p

Rolling resistance: an angular impulse against the relative spin, bounded by the normal impulse.

let
resistance
arefrolling-resistancesc
declare
single-floatresistance
when
pluspresistance
let*
rolling-mass
arefrolling-massesc
ox
arefrolling-impulses-xc
oy
arefrolling-impulses-yc
oz
arefrolling-impulses-zc
nx2
-ox
*rolling-mass
-wbxwax
ny2
-oy
*rolling-mass
-wbyway
nz2
-oz
*rolling-mass
-wbzwaz
limit
*resistancenew
magnitude-squared
+
*nx2nx2
*ny2ny2
*nz2nz2
declare
single-floatrolling-massoxoyoznx2ny2nz2limitmagnitude-squared
when
>magnitude-squared
+
*limitlimit
1e-12
let
scale
/limit
sqrt
the
single-float0f0
magnitude-squared
declare
single-floatscale
setfnx2ny2nz2
setf
arefrolling-impulses-xc
nx2
arefrolling-impulses-yc
ny2
arefrolling-impulses-zc
nz2
let
ax
-nx2ox
ay
-ny2oy
az
-nz2oz
declare
single-floataxayaz
setfwax
-wax
*iiaax
way
-way
*iiaay
waz
-waz
*iiaaz
wbx
+wbx
*iibax
wby
+wby
*iibay
wbz
+wbz
*iibaz

Friction: one tangent impulse pair, bounded by the cone.

let*
t1x
areft1xsc
t1y
areft1ysc
t1z
areft1zsc
t2x
areft2xsc
t2y
areft2ysc
t2z
areft2zsc
vrax
+vax
-
*wayraz
*wazray
vray
+vay
-
*wazrax
*waxraz
vraz
+vaz
-
*waxray
*wayrax
vrbx
+vbxkvx
-
*wbyrbz
*wbzrby
vrby
+vbykvy
-
*wbzrbx
*wbxrbz
vrbz
+vbzkvz
-
*wbxrby
*wbyrbx
vrx
-vrbxvrax
vry
-vrbyvray
vrz
-vrbzvraz
vt1
+
*vrxt1x
*vryt1y
*vrzt1z
vt2
+
*vrxt2x
*vryt2y
*vrzt2z
tangent-mass
areftangent-massesc
o1
areftangent-impulses-1c
o2
areftangent-impulses-2c
n1
-o1
*tangent-massvt1
n2
-o2
*tangent-massvt2
limit
*
areffrictionsc
new
length-squared
+
*n1n1
*n2n2
declare
single-floatt1xt1yt1zt2xt2yt2zvraxvrayvrazvrbxvrbyvrbzvrxvryvrzvt1vt2tangent-masso1o2n1n2limitlength-squared
when
>length-squared
*limitlimit
let
scale
/limit
sqrt
the
single-float0f0
length-squared
declare
single-floatscale
setfn1n2
setf
areftangent-impulses-1c
n1
areftangent-impulses-2c
n2
let*
d1
-n1o1
d2
-n2o2
px
+
*d1t1x
*d2t2x
py
+
*d1t1y
*d2t2y
pz
+
*d1t1z
*d2t2z
declare
single-floatd1d2pxpypz
setfvax
-vax
*imapx
vay
-vay
*imapy
vaz
-vaz
*imapz
vbx
+vbx
*imbpx
vby
+vby
*imbpy
vbz
+vbz
*imbpz
setfwax
-wax
*iia
-
*raypz
*razpy
way
-way
*iia
-
*razpx
*raxpz
waz
-waz
*iia
-
*raxpy
*raypx
wbx
+wbx
*iib
-
*rbypz
*rbzpy
wby
+wby
*iib
-
*rbzpx
*rbxpz
wbz
+wbz
*iib
-
*rbxpy
*rbypx
setf
arefvxsia
vax
arefvysia
vay
arefvzsia
vaz
arefwxsia
wax
arefwysia
way
arefwzsia
waz
arefvxsib
vbx
arefvysib
vby
arefvzsib
vbz
arefwxsib
wbx
arefwysib
wby
arefwzsib
wbz
constraints
defmethodphysics-solve-contacts
kernels
eql:scalar
constraintsawakestartsinv-huse-bias-pelapsedpush-max
declare
single-floatinv-helapsedpush-max
do-physics-colors
startendstarts
%physics-solve-contacts-scalarconstraintsawakestartendinv-huse-bias-pelapsedpush-max
constraints
defun%physics-apply-restitution-scalar
constraintsawakestartendthreshold
declare
fixnumstartend
single-floatthreshold
with-physics-kernel-columns
constraintsawake
declare
optimize
speed3
safety0
loopforcfixnumfromstartbelowenddo
let
relative
arefrelative-velocitiesc
restitution
arefrestitutionsc
declare
single-floatrelativerestitution

Only a real hit bounces: fast enough, and it pushed.

when
and
plusprestitution
<relative
-threshold
plusp
areftotal-normal-impulsesc
let*
ia
arefbody-asc
ib
arefbody-bsc
nx
arefnxsc
ny
arefnysc
nz
arefnzsc
rax
arefraxsc
ray
arefraysc
raz
arefrazsc
rbx
arefrbxsc
rby
arefrbysc
rbz
arefrbzsc
kvx
arefkvxsc
kvy
arefkvysc
kvz
arefkvzsc
ima
arefinverse-massesia
imb
arefinverse-massesib
iia
arefinverse-inertiasia
iib
arefinverse-inertiasib
vax
arefvxsia
vay
arefvysia
vaz
arefvzsia
wax
arefwxsia
way
arefwysia
waz
arefwzsia
vbx
arefvxsib
vby
arefvysib
vbz
arefvzsib
wbx
arefwxsib
wby
arefwysib
wbz
arefwzsib
vrax
+vax
-
*wayraz
*wazray
vray
+vay
-
*wazrax
*waxraz
vraz
+vaz
-
*waxray
*wayrax
vrbx
+vbxkvx
-
*wbyrbz
*wbzrby
vrby
+vbykvy
-
*wbzrbx
*wbxrbz
vrbz
+vbzkvz
-
*wbxrby
*wbyrbx
vn
+
*
-vrbxvrax
nx
*
-vrbyvray
ny
*
-vrbzvraz
nz
old
arefnormal-impulsesc
delta
-
*
arefnormal-massesc
+vn
*restitutionrelative
new
max
+olddelta
0f0
applied
-newold
px
*appliednx
py
*appliedny
pz
*appliednz
declare
fixnumiaib
single-floatnxnynzraxrayrazrbxrbyrbzkvxkvykvzimaimbiiaiibvaxvayvazwaxwaywazvbxvbyvbzwbxwbywbzvraxvrayvrazvrbxvrbyvrbzvnolddeltanewappliedpxpypz
setf
arefnormal-impulsesc
new
areftotal-normal-impulsesc
+
areftotal-normal-impulsesc
new
setf
arefvxsia
-vax
*imapx
arefvysia
-vay
*imapy
arefvzsia
-vaz
*imapz
arefvxsib
+vbx
*imbpx
arefvysib
+vby
*imbpy
arefvzsib
+vbz
*imbpz
setf
arefwxsia
-wax
*iia
-
*raypz
*razpy
arefwysia
-way
*iia
-
*razpx
*raxpz
arefwzsia
-waz
*iia
-
*raxpy
*raypx
arefwxsib
+wbx
*iib
-
*rbypz
*rbzpy
arefwysib
+wby
*iib
-
*rbzpx
*rbxpz
arefwzsib
+wbz
*iib
-
*rbxpy
*rbypx
constraints
defmethodphysics-apply-restitution
kernels
eql:scalar
constraintsawakestartsthreshold
declare
single-floatthreshold
do-physics-colors
startendstarts
%physics-apply-restitution-scalarconstraintsawakestartendthreshold
constraints

--------------------------------------------------------------------- Storing impulses back, with the touching state machine and hit events.

defunstore-physics-impulses
world
let*
constraints
physics-world-constraintsworld
contacts
physics-world-contactsworld
events
physics-world-eventsworld
awake
physics-world-awakeworld
hit-speed
coerce*physics-hit-speed*'single-float
declare
single-floathit-speed
optimize
speed3
safety1
records:with-columnar-buffer-storage
countrow
rowscontact
body-asbody-a
normal-impulsesnormal-impulse
tangent-impulses-1tangent-impulse-1
tangent-impulses-2tangent-impulse-2
rolling-impulses-xrolling-impulse-x
rolling-impulses-yrolling-impulse-y
rolling-impulses-zrolling-impulse-z
totalstotal-normal-impulse
relative-velocitiesrelative-velocity
constraintsphysics-constraint-columns
declare
ignorerow
dotimes
ccount
let*
row
arefrowsc
total
areftotalsc
touching
if
plusptotal
10
was-touching
arefrow
declare
fixnumrowtouchingwas-touching
single-floattotal
setf
aref
%lanephysics-contact-columnscontactsnormal-impulse
row
arefnormal-impulsesc
aref
%lanephysics-contact-columnscontactstangent-impulse-1
row
areftangent-impulses-1c
aref
%lanephysics-contact-columnscontactstangent-impulse-2
row
areftangent-impulses-2c
aref
%lanephysics-contact-columnscontactsrolling-impulse-x
row
arefrolling-impulses-xc
aref
%lanephysics-contact-columnscontactsrolling-impulse-y
row
arefrolling-impulses-yc
aref
%lanephysics-contact-columnscontactsrolling-impulse-z
row
arefrolling-impulses-zc
arefrow
touching
unless
=touchingwas-touching
physics-event-columns-pushevents
if
plusptouching
:begin:end
arefrow
arefrow
arefrow
-
arefrelative-velocitiesc

A hit: it pushed, and it arrived fast, and someone wants to know.

when
and
plusptotal
<
arefrelative-velocitiesc
-hit-speed
or
arefrow
logtest
aref
%lanephysics-body-columnsawakeflags
arefbody-asc
+physics-body-hit-report+
physics-event-columns-pushevents:hit
arefrow
arefrow
arefrow
-
arefrelative-velocitiesc
world

--------------------------------------------------------------------- Finalizing a step: fold the deltas into positions, age the mortal, and put the still to sleep.

defunfinalize-physics-bodies
worlddt
declare
single-floatdt
let*
awake
physics-world-awakeworld
events
physics-world-eventsworld
sleep-speed
coerce*physics-sleep-speed*'single-float
sleep-seconds
coerce*physics-sleep-seconds*'single-float
inverse-dt
/dt
expirednil
sleepynil
declare
single-floatsleep-speedsleep-secondsinverse-dt
records:with-columnar-buffer-storage
countrow
xsx
ysy
zsz
dxsdx
dysdy
dzsdz
vxsvx
vysvy
vzsvz
wxswx
wyswy
wzswz
radiiradius
lifetimeslifetime
sleep-timessleep-time
flagsflags
handleshandle
awakephysics-body-columns
declare
ignorerow
optimize
speed3
safety0
dotimes
icount
let*
dx
arefdxsi
dy
arefdysi
dz
arefdzsi
vx
arefvxsi
vy
arefvysi
vz
arefvzsi
wx
arefwxsi
wy
arefwysi
wz
arefwzsi
speed
+
sqrt
the
single-float0f0
+
*vxvx
*vyvy
*vzvz
*
arefradiii
sqrt
the
single-float0f0
+
*wxwx
*wywy
*wzwz
correction
*0.5f0inverse-dt
sqrt
the
single-float0f0
+
*dxdx
*dydy
*dzdz
sleep-velocity
maxspeedcorrection
declare
single-floatdxdydzvxvyvzwxwywzspeedcorrectionsleep-velocity
setf
arefxsi
+
arefxsi
dx
arefysi
+
arefysi
dy
arefzsi
+
arefzsi
dz
arefdxsi
0f0
arefdysi
0f0
arefdzsi
0f0
let
lifetime
areflifetimesi
declare
single-floatlifetime
when
>=lifetime0f0
let
left
-lifetimedt
setf
areflifetimesi
left
when
minuspleft
push
arefhandlesi
expired
if
or
>sleep-velocitysleep-speed
setf
arefsleep-timesi
0f0
let
slept
+
arefsleep-timesi
dt
declare
single-floatslept
setf
arefsleep-timesi
slept
when
>sleptsleep-seconds
push
arefhandlesi
sleepy

The sleepers age too, or a mortal body could sleep for ever.

records:with-columnar-buffer-storage
countrow
lifetimeslifetime
handleshandle
physics-world-sleepingworld
physics-body-columns
declare
ignorerow
dotimes
icount
let
lifetime
areflifetimesi
when
>=lifetime0f0
let
left
-lifetimedt
setf
areflifetimesi
left
when
minuspleft
push
arefhandlesi
expired

Relocations after the loop: they move rows.

dolist
handleexpired
physics-event-columns-pushevents:expiredhandle+physics-no-body+nil0f00f00f00f0
dolist
handlesleepy
when
world
defunwake-physics-bodies-near
worldxyzradius

Wake every sleeping body within radius of X,Y,Z: the ground moved.

let
sleeping
physics-world-sleepingworld
x
coercex'single-float
y
coercey'single-float
z
coercez'single-float
radius
coerceradius'single-float
wokennil
records:with-columnar-buffer-storage
countrow
xsx
ysy
zsz
radiiradius
handleshandle
sleepingphysics-body-columns
declare
ignorerow
dotimes
icount
let
dx
-
arefxsi
x
dy
-
arefysi
y
dz
-
arefzsi
z
limit
+radius
arefradiii
when
<
+
*dxdx
*dydy
*dzdz
*limitlimit
push
arefhandlesi
woken
dolist
handlewoken
lengthwoken

--------------------------------------------------------------------- The step.

defun%write-physics-dummy-body
world

Zero the row past the awake set's end: the static side of every contact.

let*
awake
physics-world-awakeworld
dummy
physics-body-columns-lengthawake
when
>=dummy
physics-body-columns-capacityawake
%physics-body-columns-growawake
1+dummy
macrolet
zero
&restlanes
`
setf,@
loopforlaneinlanesappend`
aref
,
intern
formatnil"PHYSICS-BODY-COLUMNS-~A-LANE"lane
awake
dummy
0f0
zerovxvyvzwxwywzdxdydzinverse-massinverse-inertiaradiusrestitutionfrictionrolling-resistance
world
defunstep-physics-world
world&optionaldt

Advance world by dt seconds (its step by default) and return it. Events from the step are then readable until the next step begins. #IDVK7G

let*
dt
coerce
ordt
physics-world-step-secondsworld
'single-float
h
/dtsubsteps
inv-h
/h
kernels
physics-world-kernelsworld
awake
physics-world-awakeworld
constraints
physics-world-constraintsworld
starts
physics-world-color-startsworld
push-max
threshold
started
get-internal-real-time
declare
single-floatdthinv-hpush-maxthreshold
fixnumsubsteps
physics-event-columns-reset
physics-world-eventsworld
incf
physics-world-step-countworld
reset-terrain-probe
physics-world-probeworld
physics-world-terrainworld

Contacts.

let

The Soft Step loop.

dotimes
substepsubsteps
let
elapsed
*hsubstep
declare
single-floatelapsed
physics-warm-startkernelsconstraintsawakestarts
physics-solve-contactskernelsconstraintsawakestartsinv-htelapsedpush-max
physics-solve-contactskernelsconstraintsawakestartsinv-hnil
+elapsedh
push-max
physics-apply-restitutionkernelsconstraintsawakestartsthreshold
setf
physics-world-last-step-contact-countworld
physics-constraint-columns-lengthconstraints
physics-world-last-step-color-countworld
color-count
setf
physics-world-last-step-real-secondsworld
/
-
get-internal-real-time
started
coerceinternal-time-units-per-second'double-float
world

--------------------------------------------------------------------- Reading the events and the world.

defmacrodo-physics-events
kindhandle-ahandle-bownerxyzspeedworld
&bodybody

Run body for each event of WORLD's last step.

let
events
gensym"EVENTS"
i
gensym"I"
`
let
,events
physics-world-events,world
dotimes
,i
physics-event-columns-length,events
let
,kind
aref
physics-event-columns-kind-lane,events
,i
,handle-a
aref
physics-event-columns-handle-a-lane,events
,i
,handle-b
aref
physics-event-columns-handle-b-lane,events
,i
,owner
aref
physics-event-columns-owner-lane,events
,i
,x
aref
physics-event-columns-x-lane,events
,i
,y
aref
physics-event-columns-y-lane,events
,i
,z
aref
physics-event-columns-z-lane,events
,i
,speed
aref
physics-event-columns-speed-lane,events
,i
declare
ignorable,kind,handle-a,handle-b,owner,x,y,z,speed
,@body
defunphysics-events
world

The last step's events as a list of plists, for inspection.

let
resultnil
do-physics-events
kindabownerxyzspeedworld
push
list:kindkind:aa:bb:ownerowner:xx:yy:zz:speedspeed
result
nreverseresult
defunphysics-world-state-hash
world

A hash of every body's position and velocity, in set and row order, so two runs of the same code can be compared. Same-image reproducibility is the claim (#S6T2MV); this is how it is policed.

let
hash0
declare
type
unsigned-byte62
hash
flet
mix
value
setfhash
logand
+
*hash1099511628211
sb-kernel:single-float-bits
coercevalue'single-float
#x3fffffffffffffff
dolist
columns
list
physics-world-awakeworld
physics-world-sleepingworld
records:with-columnar-buffer-storage
countrow
xsx
ysy
zsz
vxsvx
vysvy
vzsvz
columnsphysics-body-columns
declare
ignorerow
dotimes
icount
mix
arefxsi
mix
arefysi
mix
arefzsi
mix
arefvxsi
mix
arefvysi
mix
arefvzsi
hash
defunvalidate-physics-world
world

Check every forwarding address, the way B3VALIDATESOLVERSETS does: each live id names a row whose handle names it back, and each contact row is where its key says. Signal on the first inconsistency; return T.

let
ids
physics-world-idsworld
dotimes
index
physics-id-table-nextids
let
set
aref
physics-id-table-setids
index
unless
let*
local
aref
physics-id-table-localids
index
unless
<local
physics-body-columns-lengthcolumns
error"Body ~D points past its set: ~D."indexlocal
let
handle
aref
physics-body-columns-handle-lanecolumns
local
unless
error"Body ~D's row ~D in set ~D belongs to ~D."indexlocalset
dolist
columns
list
physics-world-awakeworld
physics-world-sleepingworld
dotimes
local
physics-body-columns-lengthcolumns
let
handle
aref
physics-body-columns-handle-lanecolumns
local
unless
error"Row ~D holds a dead handle ~D."localhandle
multiple-value-bind
setindex
unless
and
=indexlocal
error"Handle ~D at row ~D is filed at ~D/~D."handlelocalsetindex
let
contacts
physics-world-contactsworld
index
physics-world-contact-indexworld
dotimes
row
physics-contact-columns-lengthcontacts
unless
eqlrow
gethash
aref
physics-contact-columns-key-lanecontacts
row
index
error"Contact row ~D is not indexed by its key."row
unless
=
hash-table-countindex
physics-contact-columns-lengthcontacts
error"The contact index has ~D entries for ~D rows."
hash-table-countindex
physics-contact-columns-lengthcontacts
t