The functions in our DAABBTree class can sorta be divided into two categories.

The first group are the methods involved with inserting and deleting spheres from the tree.

The second group are the methods involved with querying, updating, splitting and fusing the tree.

The distinction is made clearer by noting that the tree is only 'refined' during actual QUERIES - which means that the tree adapts most quickly in the subspaces which are actively being queried.

The assumption is that although the contents of the world may be moving around, the overall structure of the tree doesn't change a whole lot from one timestep to the next - we are taking advantage of 'temporal coherence' :)

So when we add spheres to the tree (or delete them), we won't actually modify the structure of the tree, we'll instead mark one or more nodes for recalculation / refinements, which will only occur when they are reached during actual queries of the tree :)

Also, we'll be using our BBNode object in two ways.. one is for the structure of the tree, and the other is purely as a container for a single RigidBody... that is to say, we are going to wrap each RigidBody in a BBNode which is not part of the tree structure itself... we have structural nodes (the children), and we have entity nodes (the bodies).

When we add a RigidBody to the Simulator, we'll calculate its boundingsphere, create a BBNode representing that boundingsphere, and then use one of our BBTree entry-level methods to add that 'body node' to the Simulator's DAABBTree.

Any structural node can hold any number of bodies.

The 'body nodes' can and will move around the tree dynamically when the tree is queried.

This in turn causes the tree to alter its structure - in the queried subtrees, when they are queried.

Our BroadPhase Collision Test will be to walk our theoretical spheres down the tree, performing a sphere-sphere sweep test against any BodySphere nodes encountered during that traversal, and recording potential-collision records for any "intersecting time-capsules" (extruded sphere-pairs).

Attached is the more-or-less final version of the headers for the BBTree and BBNode classes.

I'll be posting all the code over the following few posts :)

When we add a RigidBody to the Simulator, we'll calculate its boundingsphere, create a BBNode representing that boundingsphere, and then use one of our BBTree entry-level methods to add that 'body node' to the Simulator's DAABBTree.

Any structural node can hold any number of bodies.

The 'body nodes' can and will move around the tree dynamically when the tree is queried.

This in turn causes the tree to alter its structure - in the queried subtrees, when they are queried.

Our BroadPhase Collision Test will be to walk our theoretical spheres down the tree, performing a sphere-sphere sweep test against any BodySphere nodes encountered during that traversal, and recording potential-collision records for any "intersecting time-capsules" (extruded sphere-pairs).

Attached is the more-or-less final version of the headers for the BBTree and BBNode classes.

I'll be posting all the code over the following few posts :)

; 覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧

UNDEFINED equ -1

WEIGHTED_BY_RADII equ TRUE ;See DoSplit method

.data

SPLITVAL dd 50 ;Best value for icoseptree according to some guys sphere-based research model

.code

; 覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧

;The BBNode class implements the nodes of a Dynamic Axially-Aligned BoundingBox (DAABB) Tree

;which has been optimized specifically to store Sphere entities for BroadPhase collision detection.

Object BBNode, 345342, Primer

RedefineMethod Init, Pointer

RedefineMethod Done

StaticMethod DoSplit ;Partition a Node

StaticMethod Contains, Pointer, Pointer ;Test if a Node's AABB encloses a given Box

StaticMethod FindNode, Pointer, real8 ;Find the node who should parent a given Sphere

StaticMethod GetExtents, Pointer, Pointer, BOOL ;Get Node's AABB, recalculating it if necessary

StaticMethod GrowExtents,Pointer, Pointer ;Grow Node's AABB to enclose given Box

StaticMethod ShrinkExtents,Pointer,real8 ;Shrink Node's AABB to discard given Sphere

StaticMethod InsertNode, Pointer ;Attach a Body-bearing Node

StaticMethod RemoveChild, Pointer ;Remove given Child Node

StaticMethod RemoveNode, Pointer, Pointer, real8 ;Detach a Body-bearing Node

StaticMethod TestSuicide ;Check if node can be destroyed

StaticMethod WhichChild, Pointer, real8 ;Choose a subspace for given sphere

StaticMethod Unsplit ;Fuse node's subspaces

StaticMethod UpdateExtents ;Recalculate node's AABB

StaticMethod UpdateNode, Pointer, real8 ;Physics calls this when entity position changed

DefineVariable bIsSplit,BOOL, FALSE ;Indicates whether this node's AABB has been subdivided

DefineVariable bExtentsAreCorrect,BOOL,FALSE ;Indicates whether this node's AABB size has changed

DefineVariable vMin, Vec3, {<>} ;Current extents of this node's AABB

DefineVariable vMax, Vec3, {<>} ; " " " " "

DefineVariable vPos, Vec3, {<>} ;Current Origin of this node's AABB (midpoint of its Extents)

DefineVariable vSplit, Vec3, {<>} ;'Weighted MidPoint' at which the Node's AABB was "split" into subspaces

DefineVariable fRadius, real8, 1.0e-12 ;Half-Width of this Node's AABB, or Radius of Entity BoundingSphere

DefineVariable children,Pointer, 27 dup <(NULL)> ;array of ptrs to child nodes (ie subtrees, branches, as you prefer)

DefineVariable childcount, dword, 27

DefineVariable pRigidBody,Pointer,NULL ;Special pointer to my entity class, not referenced by this class

Embed bodies, Collection ;holds array of BoundingSphere Nodes

ObjectEnd

LPBBNODE typedef ptr BBNode

; 覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧

;The BBTree class is mostly just a container for the Root Node of our DAABB tree.

;It implements some "entry-level" methods for recursive functions.

Object BBTree, 234566, Primer

RedefineMethod Init, Pointer

RedefineMethod Done

StaticMethod AddNode, Pointer

StaticMethod SubtractNode, Pointer

Private

StaticMethod FindNode, Pointer

Embed RootNode, BBNode

PrivateEnd

ObjectEnd

As you can see, each of these classes contains one embedded class object of another kind.

BBTree contains an embedded BBNode as its Root Node - our tree will always contain at least one valid node, even if it contains no bodies. BBNode contains an embedded Collection used to store the 'body nodes'.. remember that our tree can store those at any structural node! For non-structural nodes (where pRigidBody is not NULL) we simply don't initialize the 'bodies' collection.

ObjAsm requires that we initialize and finalize any Embedded objects ourselves.

You'll see how I do that in the following Constructor / Destructor methods:

Method BBTree.Init, uses esi, pOwner

SetObject esi

ACall Init, pOwner

OCall .RootNode::BBNode.Init, NULL

MethodEnd

Method BBTree.Done

SetObject EDX

OCall .RootNode::BBNode.Done

MethodEnd

; 覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧

;Method: BBNode.Init

;Purpose: Initialize BBNode object

;Args: pOwnerParent -> BBNode which is the immediate parent of this node

;Returns: nothing

Method BBNode.Init, uses esi, pOwnerParent

SetObject esi

;Set the Parent (Owner) pointer, this is important for us to be able to "walk up the tree"

ACall Init, pOwnerParent

;We won't initialize Child storage yet ..

;We will prepare the node for holding any amount of local entities

.if .bodies.pItems==0 && .pRigidBody==0

OCall .bodies ::Collection.Init, esi, 16, 256, UNDEFINED

.endif

OCall esi.Unsplit

MethodEnd

;Method: BBNode.Done

;Purpose: Destroy this node, its contents, its subtrees, etc

Method BBNode.Done, uses esi edi ebx

SetObject esi

;Release this node's resource objects

xor ebx,ebx

lea edi, .children

.while ebx<27

mov eax,dword ptr

mov dword ptr,0

.if eax!=0

Destroy eax ;subtrees will be destroyed recursively, this is automated by ObjAsm

.endif

inc ebx

.endw

OCall .bodies ::Collection.Done

MethodEnd

Attached are the handful of remaining BBTree methods.

We can think of these as our 'external interface methods' - these are the ones that our application (or in our case, the Simulator) will be most likely to deal with.

AddNode is used to add new 'body nodes' to the Tree.

SubtractNode is used to remove them.

FindNode is the most interesting - unfortunately, we can see that it just calls into a BBNode method of the same name, and in the context of the Root Node.. we'll have to wait to see what's in there.

The BBTree class can be seen not just as a container for the root node, but also as a kind of 'black box' to hide the complexities of the BBNode class from the user.

We can think of these as our 'external interface methods' - these are the ones that our application (or in our case, the Simulator) will be most likely to deal with.

AddNode is used to add new 'body nodes' to the Tree.

SubtractNode is used to remove them.

FindNode is the most interesting - unfortunately, we can see that it just calls into a BBNode method of the same name, and in the context of the Root Node.. we'll have to wait to see what's in there.

The BBTree class can be seen not just as a container for the root node, but also as a kind of 'black box' to hide the complexities of the BBNode class from the user.

; 覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧

;Method: BBTree.AddNode

;Purpose: TopLevel EntryPoint for inserting a new Sphere into the DAABB tree

;Args: pNode -> BBNode containing valid vPos and fRadius fields describing a BoundingSphere

;Remarks: Only need to set up Radius and Position (and pRigidBody), everything else is done for you.

Method BBTree.AddNode,uses esi ebx,pNode

local posMax:Vec3

LOCAL posMin:Vec3

SetObject esi

;Find the Node which rightfully should be the Parent of the given Node,

;noting that the given Node typically contains only a single Sphere

mov esi,$OCall(esi.FindNode,pNode)

.if eax==0

DbgWarning "Error - failed to find Node"

DbgHex pNode

int 3

.endif

;ReParent the Node

mov edx,pNode

mov .BBNode.pOwner,esi

;Attach the input Node to the appropriate Parent Node

OCall esi::BBNode.InsertNode,pNode

;Calculate the axial minima and maxima which describe

;the tightest fitting AABB that encloses the given sphere

mov edx,pNode

fld .BBNode.vPos.x

fadd .BBNode.fRadius

fstp .BBNode.vMax.x

fld .BBNode.vPos.y

fadd .BBNode.fRadius

fstp .BBNode.vMax.y

fld .BBNode.vPos.z

fadd .BBNode.fRadius

fstp .BBNode.vMax.z

fld .BBNode.vPos.x

fsub .BBNode.fRadius

fstp .BBNode.vMin.x

fld .BBNode.vPos.y

fsub .BBNode.fRadius

fstp .BBNode.vMin.y

fld .BBNode.vPos.z

fsub .BBNode.fRadius

fstp .BBNode.vMin.z

;Make sure that the Parent node encloses this new AABB

OCall esi::BBNode.GrowExtents,addr .BBNode.vMin,addr .BBNode.vMax

;Return the Parent which received the input Node

mov eax, esi

MethodEnd

;Method: BBTree.SubtractNode

;Purpose: TopLevel EntryPoint for deleting a Sphere completely from the DAABB tree

;Args: pNode -> BBNode representing Sphere to be Destroyed

;Remarks: This method eliminates a BoundingSphere (body) node from the Tree

Method BBTree.SubtractNode,uses esi , pNode

;Determine the Parent which owns the given Node

mov edx,pNode

.if .BBNode.pOwner!=0

mov esi,.BBNode.pOwner

.else

SetObject esi

mov esi,$OCall (esi.FindNode,pNode)

.endif

;Remove the given Node from its Parent

OCall esi::BBNode.RemoveNode, pNode, addr .BBNode.vPos, .BBNode.fRadius

;Trash the given node

Destroy pNode

MethodEnd

;Method: BBTree.FindNode

;Purpose: TopLevel Entrypoint to determine which node should receive the given node

;Args: pNode -> BBNode (which is a container for a single Sphere)

;Returns: EAX -> BBNode which rightfully contains pNode, or NULL = bad args

Method BBTree.FindNode, uses esi, pNode

SetObject esi

mov edx,pNode

.if edx!=0

OCall .RootNode::BBNode.FindNode,addr .BBNode.vPos, .BBNode.fRadius

.else

mov eax,edx

.endif

MethodEnd

Now we can get to the good stuff in the BBNode class...

The InsertNode method is used to attach an orphan 'body node' to a parent 'structural node'... pretty straightforward.

The FindNode method is a very good example of LINEAR RECURSION aka 'TRAVERSAL'.

We will perform a heuristic-guided search into the depths of the Tree, 'depth-walking' the structural nodes, until we find a Node which represents the best-fitting Parent for the given Theoretical Sphere.

Note that our BoundingTree is thus formed according to our volume-based heuristic, implemented in the WhichChild method (the very first one I showed you).

However that was a lot of changes ago so I'll update that method in my next post.

The InsertNode method is used to attach an orphan 'body node' to a parent 'structural node'... pretty straightforward.

The FindNode method is a very good example of LINEAR RECURSION aka 'TRAVERSAL'.

We will perform a heuristic-guided search into the depths of the Tree, 'depth-walking' the structural nodes, until we find a Node which represents the best-fitting Parent for the given Theoretical Sphere.

Note that our BoundingTree is thus formed according to our volume-based heuristic, implemented in the WhichChild method (the very first one I showed you).

However that was a lot of changes ago so I'll update that method in my next post.

;Method: BBNode.InsertNode

;Purpose: Attach the input SphereNode to this Parent structural node

Method BBNode.InsertNode,uses esi,pNode

SetObject esi

OCall .bodies::Collection.Insert,pNode

OCall pNode::BBNode.Init, esi ;Set the input Node's Parent

MethodEnd

;Method: BBNode.FindNode

;Purpose: Recursive function to search for the structural node which rightfully should own the given Sphere

Method BBNode.FindNode,uses esi ebx, pos, r:real8

SetObject esi

mov ebx,$OCall(esi.WhichChild,pos, r)

.while ebx >= 0 && .children!=0

mov esi,.children

mov ebx,$OCall(esi.WhichChild,pos, r)

.endw

mov eax,esi

MethodEnd

Attached is the revised BBNode.WhichChild method, and a bunch of math macros that I wrote as helpers for this and several other methods in this class. It implements the Icoseptree heuristic.

;Is val1-valsub <= val2 ???

$IsSubLessEq macro val1:req, valsub:req, val2:req

local @@1, @@2

fld val1

fsub valsub

fcomp val2

fjg @@1

mov eax, TRUE

jmp @@2

@@1:

xor eax, eax

@@2:

exitm <eax>

endm

;Is val1-valsub < val2 ???

$IsSubLess macro val1:req, valsub:req, val2:req

local @@1, @@2

fld val1

fsub valsub

fcomp val2

fjge @@1

mov eax, TRUE

jmp @@2

@@1:

xor eax, eax

@@2:

exitm <eax>

endm

;Is val1+valsub < val2 ???

$IsAddLess macro val1:req, valsub:req, val2:req

local @@1, @@2

fld val1

fadd valsub

fcomp val2

fjge @@1

mov eax, TRUE

jmp @@2

@@1:

xor eax, eax

@@2:

exitm <eax>

endm

;Is val1+valadd >= val2 ???

$IsAddGreaterEq macro val1:req, valadd:req, val2:req

local @@1, @@2

fld val1

fadd valadd

fcomp val2

fjl @@1

mov eax, TRUE

jmp @@2

@@1:

xor eax, eax

@@2:

exitm <eax>

endm

;Is val1+valadd > val2 ???

$IsAddGreater macro val1:req, valadd:req, val2:req

local @@1, @@2

fld val1

fadd valadd

fcomp val2

fjle @@1

mov eax,TRUE

jmp @@2

@@1:

xor eax, eax

@@2:

exitm <eax>

endm

;Is val1-valsub > val2 ???

$IsSubGreater macro val1:req, valsub:req, val2:req

local @@1, @@2

fld val1

fsub valsub

fcomp val2

fjle @@1

mov eax,TRUE

jmp @@2

@@1:

xor eax, eax

@@2:

exitm <eax>

endm

MakeMin macro output,f1,f2

fMin f1,f2

fstp output

endm

MakeMax macro output,f1,f2

fMax f1,f2

fstp output

endm

;覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧

;覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧

;Method: BBNode.WhichChild

;Purpose: Determine which Icoseptree Child rightfully owns the given SPHERE (origin, radius)

Method BBNode.WhichChild,uses esi, pos, radius:real8

LOCAL _c,sv:real8,bv:real8

SetObject esi

;If this is a Leaf node, we HAVE to leave the sphere here in this node, theres no choice

;We might decide to SPLIT this node, but we sure won't do it NOW - we'll defer that decision.

.if !.bIsSplit

return -1

.endif

and _c,0

; If the sphere痴 volume is at least 1/8 of this node's AABB volume

; then we'll just leave the sphere here in this node

.if .bExtentsAreCorrect ;better only do this when the extents are good

fldpi ;volume of sphere

fmul r4_4

fdiv r4_3

fmul radius

fstp sv ;=4/3 pi r

fld .vMax.x ;volume of box / 8

fsub .vMin.x

fld .vMax.y

fsub .vMin.y

fld .vMax.z

fsub .vMin.z

fmul

fmul

fdiv r4_8

fstp bv ;= (length*width*height) / 8

.if $IsGreater(sv, bv)

return -1 ;"leave it here"

.endif

.endif

;ICOSEPTREE CHILD SELECTION:

;The following three tests will yield a value from 0 to 26 inclusive (ie 27 possible values)

;This indicates which Icoseptree Child owns the input sphere

mov edx,pos

.if $IsAddLess(.Vec3.x, radius, .vSplit.x)

add _c,1

.else

.if $IsSubGreater(.Vec3.x, radius, .vSplit.x)

add _c,2

.endif

.endif

.if $IsAddLess(.Vec3.y, radius, .vSplit.y)

add _c,3

.else

.if $IsSubGreater(.Vec3.y, radius, .vSplit.y)

add _c,6

.endif

.endif

.if $IsAddLess(.Vec3.z, radius, .vSplit.z)

add _c,9

.else

.if $IsSubGreater(.Vec3.z, radius, .vSplit.z)

add _c,18

.endif

.endif

;Assert that the indicated child index is valid in this node

mov eax,_c

.if .children==0

DbgWarning "Error in BBNode.WhichChild - the indicated child node does not exist"

DbgDec _c,"Icoseptree child index"

;We COULD take this opportunity to create the necessary Child Node,

;but since its NOT a leaf, we expected it to already have been done?

int 3

.endif

MethodEnd

The first thing we'll look at is detaching a BoundingSphere node from a structural node.

It may not seem intuitive that we're starting here, but it is the most simple function and can be understood without much context (I think).

RemoveNode detaches a given BoundingSphere node from the Tree, but it does not destroy the object.

This method is used later to manage the dynamic tree. It makes a call to another method called ShrinkExtents. This method's name is deceiving - it doesn't really shrink anything - it checks whether the given sphere is violating the bounds of the current node's AABB (box), and if so, it simply marks the node for future recalculation, and then heads UP to its Parent node and repeats the process, until it runs out of Parent nodes, or finds a node which encloses the sphere.

Note that we don't actually correct the extents of the nodes we visited here, any modifications to the tree are deferred until the tree is Queried, as we shall see.

It may not seem intuitive that we're starting here, but it is the most simple function and can be understood without much context (I think).

RemoveNode detaches a given BoundingSphere node from the Tree, but it does not destroy the object.

This method is used later to manage the dynamic tree. It makes a call to another method called ShrinkExtents. This method's name is deceiving - it doesn't really shrink anything - it checks whether the given sphere is violating the bounds of the current node's AABB (box), and if so, it simply marks the node for future recalculation, and then heads UP to its Parent node and repeats the process, until it runs out of Parent nodes, or finds a node which encloses the sphere.

Note that we don't actually correct the extents of the nodes we visited here, any modifications to the tree are deferred until the tree is Queried, as we shall see.

;Method: BBNode.RemoveNode

;Purpose: Detach (not destroy) given 'bodynode' from THIS Parent node

;Args: pNode -> BoundingSphere Node

;Returns: EAX = pNode

Method BBNode.RemoveNode,uses esi, pNode

SetObject esi

;Check if we can reduce the size of this node (and if so , also its parents)

;Such nodes will be MARKED for resizing, but we won't do it now...

mov edx,pNode

OCall esi.ShrinkExtents,addr .BBNode.vPos, .BBNode.fRadius

OCall .bodies::Collection.Delete,pNode

;Check if we can destroy this node

OCall esi.TestSuicide

mov eax, pNode

MethodEnd

;Check if the given Sphere violates this structural node's AABB.

;If so, mark the node for recalc, and check its parent node(s) as well.

;We will break as soon as we find a parent node that hasn't been violated,

;or we reached the Tree's root node (since it has no parent to check).

Method BBNode.ShrinkExtents,uses esi,pvPos, r:real8

SetObject esi

mov edx,pvPos

.while esi!=0

;Break if sphere is totally inside aabb

.if $IsSubGreater(.Vec3.x,r,.vMin.x)

.if $IsSubGreater(.Vec3.y,r,.vMin.y)

.if $IsSubGreater(.Vec3.z,r,.vMin.z)

.if $IsAddLess(.Vec3.x,r,.vMax.x)

.if $IsAddLess(.Vec3.y,r,.vMax.y)

.break .if $IsAddLess(.Vec3.z,r,.vMax.z)

.endif

.endif

.endif

.endif

.endif

;#sphere is violating aabb

mov .bExtentsAreCorrect, FALSE

mov esi,.pOwner

.endw

MethodEnd

Here's the flip-side to the previous post: the GrowExtents method is called during Queries to grow the AABB of structural nodes which have been tagged for recalculation.

The AABB is grown such that it encloses a given Box specified by a pair of min/max Points.

Like previously, this method walks to its Parent and repeats this process until we reach a node whose AABB is valid (and needs no recalculating), or we reach the root node.

Below it is the TestSuicide method.

This is a garbage-collection method which is called to check whether a structural node is devoid of content and so can be destroyed.

The AABB is grown such that it encloses a given Box specified by a pair of min/max Points.

Like previously, this method walks to its Parent and repeats this process until we reach a node whose AABB is valid (and needs no recalculating), or we reach the root node.

Below it is the TestSuicide method.

This is a garbage-collection method which is called to check whether a structural node is devoid of content and so can be destroyed.

;If the current node is marked for recalc, enlarge it

;to enclose the given box extents (lo, hi) and check Parents

;Note that we at NO POINT mark the node as having Corrected Extents -

;We leave it marked 'uncorrect' until it is next Updated (ie, when Queried).

Method BBNode.GrowExtents,uses esi,lo,hi

SetObject esi

.while esi!=0 && !.bExtentsAreCorrect

mov edx,lo

MakeMin .vMin.x, .vMin.x, .Vec3.x

MakeMin .vMin.y, .vMin.y, .Vec3.y

MakeMin .vMin.z, .vMin.z, .Vec3.z

mov edx,hi

MakeMax .vMax.x, .vMax.x, .Vec3.x

MakeMax .vMax.y, .vMax.y, .Vec3.y

MakeMax .vMax.z, .vMax.z, .Vec3.z

mov esi,.pOwner

.endw

MethodEnd

;Method: BBNode.TestSuicide

;Purpose: Check if this node is ready to go byebye

Method BBNode.TestSuicide,uses esi ebx

SetObject esi

.if .pOwner!=0 && .bodies.dCount==0 && .childcount==0

OCall .pOwner::BBNode.RemoveChild,esi ;Destroy this node from parent context

Destroy esi

.endif

MethodEnd

Since the tree is dynamic, occasionally we'll want to destroy Structural nodes that have become redundant. The BBNode.RemoveChild method is responsible for deleting a structural node's reference from 'this' parent node, and then actually destroying it.

If RemoveChild causes the number of Child Nodes falls to zero, it calls the Unsplit method, which will mark the node as being a Leaf, and attempt to redistribute any BoundingSpheres it contains. Finally it will perform a Suicide check.

We might say that these methods are responsible for dynamically collapsing the tree structure.

If RemoveChild causes the number of Child Nodes falls to zero, it calls the Unsplit method, which will mark the node as being a Leaf, and attempt to redistribute any BoundingSpheres it contains. Finally it will perform a Suicide check.

We might say that these methods are responsible for dynamically collapsing the tree structure.

;Method: BBNode.RemoveChild

;Purpose: Remove given Child Node from this Parent Node

Method BBNode.RemoveChild,uses esi ebx, foo

LOCAL unsplit:BOOL

SetObject esi

;If this Node contains the given Child, dispose of that child

xor ebx,ebx

mov edx,foo

.while ebx<27

.if .children==edx

mov .children,0

dec .childcount

Destroy edx

;If the count of children falls to zero, Unsplit this Node

.if .childcount==0

OCall esi.Unsplit

.endif

.break

.endif

inc ebx

.endw

;If this Node has become empty and useless, destroy it

OCall esi.TestSuicide

MethodEnd

;Method: BBNode.Unsplit

;Purpose: Make this node into a Leaf by trashing its subtrees and marking it as unsplit.

; If the node contains any BoundingSpheres, throw them at the Parent Node

; They could land back in here, that is legal.

Method BBNode.Unsplit,uses esi ebx

SetObject esi

;Mark this Node as 'UNSPLIT'

mov .bIsSplit,FALSE

;If it is NOT the root node,

.if .pOwner!=0

;If this Node contains any 'bodies', detach them and throw them at this node's parent

;They could very well fall back into this Node !!

xor ebx,ebx

.while ebx<.bodies.dCount

;Grab the first available element from the list

mov edx,$OCall (.bodies::Collection.DeleteAt,0)

;Detach it (preserve on stack momentarily)

push $OCall (esi.RemoveNode, edx)

;Find the Node which rightfully owns it

mov edx,eax

OCall .pOwner::BBNode.FindNode,addr .BBNode.vPos,.BBNode.fRadius

;Reattach it to its rightful owner

pop edx ;(restore from stack)

OCall eax::BBNode.InsertNode,edx

inc ebx

.endw

.endif

MethodEnd

We're now ready to look at methods which Query and Update the tree.

The BBNode.Contains method checks whether a given Box Extents are completely enclosed by the current node's AABB.

The BBNode.Intersects method is quite similar - it tests whether a given box is (partly or completely) overlapping the current node's AABB, or whether they are separated by some distance.

Both of these methods call the GetExtents method. Calling this method can trigger a cascading recalculation of the AABB's of nodes marked as 'invalid' - we'll see that in the next post ;)

The BBNode.Contains method checks whether a given Box Extents are completely enclosed by the current node's AABB.

The BBNode.Intersects method is quite similar - it tests whether a given box is (partly or completely) overlapping the current node's AABB, or whether they are separated by some distance.

Both of these methods call the GetExtents method. Calling this method can trigger a cascading recalculation of the AABB's of nodes marked as 'invalid' - we'll see that in the next post ;)

;Method: BBNode.Contains

;Purpose: Determine if the given 'Box Extents' are within the bounds of the this Node's box

;Args: minp -> Vec3 minima

; maxp -> Vec3 maxima

;Returns: TRUE / FALSE

Method BBNode.Contains,uses esi,minp, maxp

LOCAL min:Vec3, max:Vec3

SetObject esi

;Query the extents of this Node's AABB

;This can trigger recalculation and splitting

OCall esi.GetExtents,addr min, addr max, TRUE

mov edx,minp

.if $IsGreaterOrEqual(.Vec3.x, min.x)

.if $IsGreaterOrEqual(.Vec3.y, min.y)

.if $IsGreaterOrEqual(.Vec3.z, min.z)

mov edx,maxp

.if $IsLessOrEqual(.Vec3.x, max.x)

.if $IsLessOrEqual(.Vec3.y, max.y)

.if $IsLessOrEqual(.Vec3.z, max.z)

return TRUE

.endif

.endif

.endif

.endif

.endif

.endif

xor eax,eax ;FALSE

MethodEnd

;Method: BBNode.Intersects

;Purpose: Determine if the given 'Box Extents' intersects (ie overlaps or is inside) the bounds of the this Node's box

;Args: minp -> Vec3 minima

; maxp -> Vec3 maxima

;Returns: TRUE / FALSE

Method BBNode.Intersects,uses esi,minp, maxp

LOCAL min:Vec3, max:Vec3

SetObject esi

;Query the extents of this Node's AABB

;This can trigger recalculation and splitting

OCall esi.GetExtents,addr min, addr max, TRUE

mov edx,maxp

.if $IsGreaterOrEqual(min.x,.Vec3.x) ;Is given box maxima >= node AABB minima?

.if $IsGreaterOrEqual(min.y,.Vec3.y)

.if $IsGreaterOrEqual(min.z,.Vec3.z)

mov edx,minp

.if $IsLessOrEqual(.Vec3.x, max.x) ;Is given box minima <= node AABB maxima?

.if $IsLessOrEqual(.Vec3.y, max.y)

.if $IsLessOrEqual(.Vec3.z, max.z)

return TRUE ;Box is intersecting the AABB

.endif

.endif

.endif

.endif

.endif

.endif

xor eax,eax ;FALSE

MethodEnd

;Method: BBNode.GetExtents

;Purpose: Return the extents of this node's AABB,

;Args: minp -> Vec3 to store minima

; maxp -> Vec3 to store maxima

; split : Boolean - Indicates whether or not to split nodes during the operation

;Remaarks: This is a QUERY method, and triggers a cascading recalculation on demand.

Method BBNode.GetExtents,uses esi,minp, maxp, split:BOOL

SetObject esi

.if !.bExtentsAreCorrect

OCall esi.UpdateExtents

.endif

mov edx,SPLITVAL

.if split && !.bIsSplit && .bodies.dCount >= edx

OCall esi.DoSplit

.endif

mov edx,minp

fld .vMin.x

fld .vMin.y

fld .vMin.z

fstp .Vec3.z

fstp .Vec3.y

fstp .Vec3.x

mov edx,maxp

fld .vMax.x

fld .vMax.y

fld .vMax.z

fstp .Vec3.z

fstp .Vec3.y

fstp .Vec3.x

MethodEnd

Here's the last few methods for the querying/updating of the DAABBTree - but I'm not done.

The next post will add a few new methods which are specific to Collision Detection :)

The BBNode.UpdateExtents method is called by the GetExtents method to recalculate an 'invalidated' Structural node's AABB, and set it's flag to signal 'extents are correct'.

The BBNode.DoSplit method is called by GetExtents to actually subdivide a Leaf node into 27 child nodes. This is the method responsible for the tree 'growing' dynamically during Queries.

The BBNode.UpdateNode method is called by external code once per timestep to update the position of a moving BoundingSphere node, and to adjust its position in the tree, potentially queuing tree restructuring but deferring it until Queried. Basically whenever one of our RigidBody instances has moved in 3D space, we'll call this Update method on its container node which will keep them synchronized and adjust the tree for us.

The next post will add a few new methods which are specific to Collision Detection :)

The BBNode.UpdateExtents method is called by the GetExtents method to recalculate an 'invalidated' Structural node's AABB, and set it's flag to signal 'extents are correct'.

The BBNode.DoSplit method is called by GetExtents to actually subdivide a Leaf node into 27 child nodes. This is the method responsible for the tree 'growing' dynamically during Queries.

The BBNode.UpdateNode method is called by external code once per timestep to update the position of a moving BoundingSphere node, and to adjust its position in the tree, potentially queuing tree restructuring but deferring it until Queried. Basically whenever one of our RigidBody instances has moved in 3D space, we'll call this Update method on its container node which will keep them synchronized and adjust the tree for us.

;Method: BBNode.UpdateExtents

;Purpose: Re-Calculate this node's AABB and SplitPoint based on node's Child AABB's and Body AABB's

;Remarks: SplitPoint Calculation is not the same as used initially, this bothers me

Method BBNode.UpdateExtents, uses esi ebx

LOCAL set:BOOL

LOCAL ct

LOCAL newsplit:Vec3

LOCAL lo:Vec3, hi:Vec3

SetObject esi

;Preset the local variables

mov set,FALSE

and ct,0

fldz

fst newsplit.x

fst newsplit.y

fstp newsplit.z

xor ebx,ebx

.while ebx<.bodies.dCount

OCall .bodies::Collection.ItemAt, ebx

fld .BBNode.vPos.x

fld .BBNode.vPos.y

fld .BBNode.vPos.z

fld .BBNode.fRadius

fsub st(3),st(0)

fsub st(2),st(0)

fsub;st(1),st(0)...fUnload

fstp lo.z

fstp lo.y

fstp lo.x

fld .BBNode.vPos.x

fld .BBNode.vPos.y

fld .BBNode.vPos.z

fld .BBNode.fRadius

fadd st(3),st(0)

fadd st(2),st(0)

fadd;st(1),st(0)...fUnload

fstp hi.z

fstp hi.y

fstp hi.x

.if !set

fld lo.x

fld lo.y

fld lo.z

fstp .vMin.z

fstp .vMin.y

fstp .vMin.x

fld hi.x

fld hi.y

fld hi.z

fstp .vMax.z

fstp .vMax.y

fstp .vMax.x

mov set, TRUE

.else

MakeMin .vMin.x, .vMin.x, lo.x

MakeMin .vMin.y, .vMin.y, lo.y

MakeMin .vMin.z, .vMin.z, lo.z

MakeMax .vMax.x, .vMax.x, hi.x

MakeMax .vMax.y, .vMax.y, hi.y

MakeMax .vMax.z, .vMax.z, hi.z

.endif

fld .vPos.x

fld .vPos.y

fld .vPos.z

fadd newsplit.z

fstp newsplit.z

fadd newsplit.y

fstp newsplit.y

fadd newsplit.x

fstp newsplit.x

inc ct

inc ebx

.endw

xor ebx,ebx

.while ebx<27

push ebx

mov ebx,.children

.if ebx!=0 ;Recurse each valid Child for its extents

OCall ebx::BBNode.GetExtents,addr lo, addr hi, FALSE

;Recalculate this node's AABB to include all its Child AABB's

.if !set

fld lo.x

fld lo.y

fld lo.z

fstp .vMin.z

fstp .vMin.y

fstp .vMin.x

fld hi.x

fld hi.y

fld hi.z

fstp .vMax.z

fstp .vMax.y

fstp .vMax.x

mov set, TRUE

.else

MakeMin .vMin.x, .vMin.x, lo.x

MakeMin .vMin.y, .vMin.y, lo.y

MakeMin .vMin.z, .vMin.z, lo.z

MakeMax .vMax.x, .vMax.x, hi.x

MakeMax .vMax.y, .vMax.y, hi.y

MakeMax .vMax.z, .vMax.z, hi.z

.endif

;We will sum the SplitPoints of the child nodes

fld .BBNode.vSplit.x

fld .BBNode.vSplit.y

fld .BBNode.vSplit.z

fild SPLITVAL

fmul st(3),st(0)

fmul st(2),st(0)

fmul

fadd newsplit.z

fstp newsplit.z

fadd newsplit.y

fstp newsplit.y

fadd newsplit.x

fstp newsplit.x

inc ct

.endif

pop ebx

inc ebx

.endw

;This node's splitpoint = averaged midpoint of its child

fld newsplit.x

fld newsplit.y

fld newsplit.z

fild ct

fdiv st(3),st(0)

fdiv st(2),st(0)

fdiv

fstp .vSplit.z

fstp .vSplit.y

fstp .vSplit.x

mov .bExtentsAreCorrect,TRUE

MethodEnd

;Method: BBNode.DoSplit

;Purpose: Split this node into 27 subspaces , remembering the splitpoint and childpointers

Method BBNode.DoSplit,uses esi edi ebx

LOCAL ttl:real8,rw:real8

LOCAL safe:BOOL,first:BOOL

LOCAL lc, _c

SetObject esi

.if .bIsSplit

DbgWarning "Error - Node already marked as being split"

int 3

.endif

; Find the split point (preset to zero, and calculate weighted average)

fldz

fst .vSplit.x

fst .vSplit.y

fst .vSplit.z

fstp ttl

xor ebx,ebx

.while ebx<.bodies.dCount

OCall .bodies::Collection.ItemAt,ebx

if WEIGHTED_BY_RADII

;Weighting factor = 1/Radius

;rw = 1.0/(r + 1e-12) ... where 1e-12 is basically "radius cant be zero" sanity thing

fld1

fdiv .BBNode.fRadius

else

;Weighting factor = 1

fld1

endif

fst rw

fadd ttl

fstp ttl

;vSplit += (pos + radius)*rw ;Sum of Weighted Position and Radius

fld .BBNode.vPos.x

fadd .BBNode.fRadius

fld .BBNode.vPos.y

fadd .BBNode.fRadius

fld .BBNode.vPos.z

fadd .BBNode.fRadius

fld rw

fmul st(3),st(0)

fmul st(2),st(0)

fmul

fadd .vSplit.z

fstp .vSplit.z

fadd .vSplit.y

fstp .vSplit.y

fadd .vSplit.x

fstp .vSplit.x

inc ebx

.endw

;vSplit /= ttl ;Average of (Sum of Weighted Position and Radius)

fld .vSplit.x

fld .vSplit.y

fld .vSplit.z

fld ttl

fdiv st(3),st(0)

fdiv st(2),st(0)

fdiv

fstp .vSplit.z

fstp .vSplit.y

fstp .vSplit.x

;We JUST split it so init the children

mov .bIsSplit,TRUE

mov .childcount,27

xor ebx,ebx

.while ebx<27

mov .children,$New(BBNode,Init,esi)

inc ebx

.endw

mov safe,FALSE

mov first, TRUE

mov lc,0

xor ebx,ebx

.while ebx<.bodies.dCount

push ebx

mov ebx,$OCall (.bodies::Collection.DeleteAt,0)

mov _c,$OCall (esi.WhichChild, addr .BBNode.vPos, .BBNode.fRadius)

; Make sure at least one object doesn稚 go into the same child as everyone else

.if !safe

.ifBitSet _c, BIT31

mov safe,TRUE

.else

.if first

m2m lc, _c, edx

mov first, FALSE

.else

mov edx,lc

.if _c !=edx

mov safe,TRUE ;bodies went into different children

.else

or safe,FALSE ;bodies went to same child

.endif

.endif

.endif

.endif

.ifBitSet _c, BIT31

;Put it back in this node

OCall .bodies::Collection.Insert,ebx

.else

mov edx,_c

OCall .children::BBNode.InsertNode, ebx

.endif

pop ebx

inc ebx

.endw

.if !safe

; Oops, all objects went into the same child node! Take them back.

mov edx,lc

mov edi,.children

xor ebx,ebx

.while ebx<.BBNode.bodies.dCount

mov edx,$OCall (.BBNode.bodies::Collection.ItemAt,ebx)

OCall edi::BBNode.RemoveNode,edx ;Detach from child

OCall esi.InsertNode,eax ;Attach to self

inc ebx

.endw

; The last removal marked this node as unsplit, so

; we値l just have to do this all over again next time

; this AABB is queried, UNLESS we simply keep this node split

mov .bIsSplit,TRUE

.endif

;Update the BoundingBox Extents of all Child nodes

xor ebx,ebx

.while ebx<27

.if .children!=0

OCall .children::BBNode.UpdateExtents

.endif

inc ebx

.endw

MethodEnd

;Method: BBTree.UpdateNode

;Purpose: Update the Position of a BoundingSphere node, possibly queuing changes to the tree structure

;Args: pNode = ptr to Node being processed

Method BBNode.UpdateNode,uses esi edi ebx

LOCAL minp:Vec3, maxp:Vec3

SetObject esi

mov ebx,esi

mov edi,esi

;Update the node's Position from the RigidBody

mov edx,.pRigidBody

fld .RigidBody.linPos0.x

fld .RigidBody.linPos0.y

fld .RigidBody.linPos0.z

fstp .vPos.z

fstp .vPos.y

fstp .vPos.x

; Calculate a theoretical Box at the origin of this Node, with size of +/- radius in each major axis

; This is the smallest Box that contains this Node's BOUNDING SPHERE

fld .vPos.x ;Calculate maxima

fld .vPos.y

fld .vPos.z

fld .fRadius

fadd st(3),st(0)

fadd st(2),st(0)

fadd

fstp maxp.z

fstp maxp.y

fstp maxp.x

fld .vPos.x ;Calculate minima

fld .vPos.y

fld .vPos.z

fld .fRadius

fsub st(3),st(0)

fsub st(2),st(0)

fsub

fstp minp.z

fstp minp.y

fstp minp.x

;Backtrack up the tree to the root node until we find one that does *NOT* include our Box

.while .BBNode.pOwner!=0 && !$OCall (ebx::BBNode.Contains,addr minp, addr maxp)

mov ebx,.BBNode.pOwner

.endw

; Find its final reinsertion point

OCall ebx::BBNode.FindNode,addr .BBNode.vPos,.BBNode.fRadius

.if eax!=edi

push .BBNode.pOwner ;preserve old parent

mov ebx,eax

OCall ebx::BBNode.InsertNode,esi ;Attach the node to new Parent

OCall ebx::BBNode.GrowExtents,addr minp, addr maxp ;Verify bounds of new Parent

pop edi ;restore old parent

OCall edi::BBNode.RemoveNode,esi ;Detach from old parent, will shrink extents

.endif

MethodEnd

Finally, we're back to BroadPhase Collision Detection :)

The goal of our broadphase is to find pairs of BoundingSpheres which are currently intersecting, or which are heading toward imminent collision, while ignoring pairs of spheres which cannot possibly collide.

We have created a Dynamic Tree whose nodes contain our BoundingSpheres, with larger spheres getting 'stuck' at nonleaf nodes, and smaller nodes falling further toward the leaf nodes, which can hold a maximum of (51) entities before splitting can be attempted. We'll use this tree to accelerate our broadphase collision queries.

We'll need to take each boundingsphere node, and send it down the tree from the root node.

At each node we visit, if the node contains local spheres, we'll perform a sphere-sweep test of our subject sphere and each local sphere, possibly generating 'CollisionPair Records". Then we'll choose a child to recurse, and walk to it. This will repeat until we reach a leaf node, or until the subject sphere doesn't intersect the AABB of the node.

All of this begins in the Physics Simulator, so my next post will be to begin showing the BroadPhase collision detection methods from their entrypoint there.

The goal of our broadphase is to find pairs of BoundingSpheres which are currently intersecting, or which are heading toward imminent collision, while ignoring pairs of spheres which cannot possibly collide.

We have created a Dynamic Tree whose nodes contain our BoundingSpheres, with larger spheres getting 'stuck' at nonleaf nodes, and smaller nodes falling further toward the leaf nodes, which can hold a maximum of (51) entities before splitting can be attempted. We'll use this tree to accelerate our broadphase collision queries.

We'll need to take each boundingsphere node, and send it down the tree from the root node.

At each node we visit, if the node contains local spheres, we'll perform a sphere-sweep test of our subject sphere and each local sphere, possibly generating 'CollisionPair Records". Then we'll choose a child to recurse, and walk to it. This will repeat until we reach a leaf node, or until the subject sphere doesn't intersect the AABB of the node.

All of this begins in the Physics Simulator, so my next post will be to begin showing the BroadPhase collision detection methods from their entrypoint there.

The CollisionPair structure will store a record of a pair of potentially-colliding entities. The broadphase will emit these records. My implementation will collect up to 256 CollisionPair records at once.

Here's some stuff we'll use in our broadphase:

Before we begin, some notes about collision pairs.

We will start with the convention that the pBodyA and pBodyB fields are sorted by the order of their indices in the simulator's global list of bodies. What this implies is that BodyB can never have a lower index than BodyA - this is important because it can help us eliminate some unwanted testing.

We will reinforce this convention by declaring that BodyA is the "Aggressor" and BodyB is the "Target" of the potential collision event - this implies that BodyB may be At Rest (ie sleeping) but BodyA is most definitely Awake.

Now having said that, if we were to perform a bruteforce pairwise test (which we will NOT do), we would need to process each BodyA against any bodies with a higher index than its own...Specifically, an outer loop where Body A's index counts from zero to #count-2, and an inner loop with BodyB = initial index is (BodyA's index)+1, and final index is #count-1... we'll bear in mind that we can use this logic to eliminate duplicate pairwise tests during our DAABB tree querying (code for this has not yet been presented).

So here's our entrypoint to the broadphase collision testing.

The Physics.BroadPhase_Collisions method simply passes each simulated body's bounding-node to a BBTree method for further evaluation.

I've also included the Physics.Add_Sphere method, whose purpose is to create a new RigidBody with a Spherical shape, and given physical attributes, create an associated BBNode, add the new node to the tree, and add the new RigidBody instance to the Simulator's master-list.

We'll be able to add any kind of shape wrapped in a sphere, but I'm starting with actual spheres for testing purposes, and because "if I can't do it with spheres, then I can't do it with anything more complex" ;)

Here's some stuff we'll use in our broadphase:

;CollisionDetection Values:

MAX_COLLISION_PAIRS equ 256

Clear equ 0

Penetrating equ -1

Colliding equ 1

CollisionPair struct

pBodyA Pointer ?

pBodyB Pointer ?

fLowerBound real8 ?

fUpperBound real8 ?

CollisionPair ends

CollisionPairArray struct

Collectionpairs CollisionPair MAX_COLLISION_PAIRS dup (<>)

CollisionPairArray ends

Before we begin, some notes about collision pairs.

We will start with the convention that the pBodyA and pBodyB fields are sorted by the order of their indices in the simulator's global list of bodies. What this implies is that BodyB can never have a lower index than BodyA - this is important because it can help us eliminate some unwanted testing.

We will reinforce this convention by declaring that BodyA is the "Aggressor" and BodyB is the "Target" of the potential collision event - this implies that BodyB may be At Rest (ie sleeping) but BodyA is most definitely Awake.

Now having said that, if we were to perform a bruteforce pairwise test (which we will NOT do), we would need to process each BodyA against any bodies with a higher index than its own...Specifically, an outer loop where Body A's index counts from zero to #count-2, and an inner loop with BodyB = initial index is (BodyA's index)+1, and final index is #count-1... we'll bear in mind that we can use this logic to eliminate duplicate pairwise tests during our DAABB tree querying (code for this has not yet been presented).

So here's our entrypoint to the broadphase collision testing.

The Physics.BroadPhase_Collisions method simply passes each simulated body's bounding-node to a BBTree method for further evaluation.

I've also included the Physics.Add_Sphere method, whose purpose is to create a new RigidBody with a Spherical shape, and given physical attributes, create an associated BBNode, add the new node to the tree, and add the new RigidBody instance to the Simulator's master-list.

We'll be able to add any kind of shape wrapped in a sphere, but I'm starting with actual spheres for testing purposes, and because "if I can't do it with spheres, then I can't do it with anything more complex" ;)

;Method: Physics.BroadPhase_Collisions

;Purpose: Find pairs of entities whose BoundingSpheres are currently intersecting,

; or whose swept projections will intersect during the current timestep.

Method Physics.BroadPhase_Collisions, uses esi ebx

SetObject esi

;The Physics Simulator keeps the master-list of RigidBody instances in its Ancestor Collection.

;We must enumerate this list, passing each

xor ebx,ebx

.while ebx<.dCount

OCall esi.ItemAt,ebx ;-> RigidBody

mov edx,.RigidBody.pBoundingNode ;-> BBNode

OCall .DAABBTree::BBTree.FindCollisionPairs,edx, addr .CollisionPairs, .dMaxCollisionPairs

inc ebx

.endw

MethodEnd

; Method: Physics.Add_Sphere

; Purpose: Add a new Sphere Entity to the Simulation

; Args: pvPosition -> WorldSpace Position of Center of Mass

; pvLinarVelocity -> WorldSpace Velocity of Center of Mass

; pOrientationQuaternion -> Quaternion representing Orientation (0,0,0,1 = identity)

; pvAngularVelocity -> Vector representing velocity about a given axis

; fLinearKineticDamping = Float scalar for removing kinetic energy

; fAngularKineticDamping = Float scalar for removing kinetic energy

; fGravity = Float Gravitational Constant (per Body !!)

Method Physics.Add_Sphere, uses esi ebx, pvPosition, pvLinearVelocity, pOrientationQuaternion, pvAngularVelocity, fLinearKineticDamping:real4, fAngularKineticDamping:real4, fGravity:real4, fRadius:real8, fDensity:real4

LOCAL body

LOCAL fMass:real4

SetObject esi

;Create a RigidBody from the input params

mov body, $New(RigidBody,Init, esi, pvPosition, pvLinearVelocity, pOrientationQuaternion, pvAngularVelocity,fLinearKineticDamping, fAngularKineticDamping)

;We need to calculate ehe Mass of the sphere

;We know the Density and the Radius.

;We know Mass = Density * Volume

;Volume of a Sphere is 4/3 pi r ^3

fldpi

fld fRadius

fmul st(0),st(0)

fmul st(0),st(0)

fmul

fmul r8_4Over3

fmul fDensity

fstp fMass

OCall body::RigidBody.setSphere, fRadius, fMass

OCall body::RigidBody.setGravity, fGravity

;Create a BBNode and set its Position and Radius

mov ebx,$New(BBNode)

mov eax,pvPosition

m2m .BBNode.vPos.x, .Vec3.x,edx

m2m .BBNode.vPos.y, .Vec3.y,edx

m2m .BBNode.vPos.z, .Vec3.z,edx

fld fRadius

fstp .BBNode.fRadius

;Set a special Pointer to RigidBody in this node

m2m .BBNode.pRigidBody,body,edx

mov .RigidBody.pBoundingNode,ebx

;Add the BBNode to the DAABB Tree

OCall .DAABBTree::BBTree.AddNode,ebx

;Insert RigidBody into Simulator

OCall esi.Insert,body

MethodEnd

The broadphase method leads us to the BBTree.FindCollisionPairs method.

This is simply a stub which calls the BBNode method of the same name, and in the context of the Root node of the tree.

Finally we reach the BBNode.FindCollisionPairs method, which is another good example of a Linear Traversal (rather than a Nested Recursion).

The first thing this method does is check if the current node's AABB intersects the input sphere's AABB.

If it doesn't , the method will return.

Next it checks if the current node contains any BoundingSpheres, and if so, performs a Swept Test of the input sphere and each of the node's spheres.

Finally, it checks if there are any Child nodes, and if so, chooses one child node to traverse into, and the whole process repeats until we hit a leaf node, or can't find any child volumes which intersect the input sphere.

If we perform a sweep test and the result indicates a current or pending intersection of a pair of boundingspheres, we record this CollisionPair.

What's currently missing from this function is our prior knowledge of legal pairings in CollisionPair records (remember I made a fuss about this?) : the index of pNode must be greater than the index of pBody or else the test is a waste of time (since this body pairing must already have been queried !!)

This is simply a stub which calls the BBNode method of the same name, and in the context of the Root node of the tree.

Method BBTree.FindCollisionPairs,uses esi,pNode,pRecords,dMaxRecords

SetObject esi

OCall .RootNode::BBNode.FindCollisionPairs,pNode,pRecords,dMaxRecords

MethodEnd

Finally we reach the BBNode.FindCollisionPairs method, which is another good example of a Linear Traversal (rather than a Nested Recursion).

The first thing this method does is check if the current node's AABB intersects the input sphere's AABB.

If it doesn't , the method will return.

Next it checks if the current node contains any BoundingSpheres, and if so, performs a Swept Test of the input sphere and each of the node's spheres.

Finally, it checks if there are any Child nodes, and if so, chooses one child node to traverse into, and the whole process repeats until we hit a leaf node, or can't find any child volumes which intersect the input sphere.

If we perform a sweep test and the result indicates a current or pending intersection of a pair of boundingspheres, we record this CollisionPair.

; 覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧

; 覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧覧

;Method: BBNode.FindCollisionPairs

;Purpose: Identify pairs of Spheres whose extruded capsules intersect during the current timeframe

; Return CollisionPair records up to the prescribed limit

;Args: pNode -> BoundingSphere Node being Tested against this Structural Node

Method BBNode.FindCollisionPairs,uses esi ebx,pNode, pRecords, dMaxRecords

LOCAL pBody

SetObject esi

;We will stop recursing if we reach a node whose AABB does not intersect with the Sphere

@@:

mov edx,pNode

.if $OCall (esi.Intersects, addr .BBNode.vMin,addr .BBNode.vMax)== TRUE

;perform Swept Spheres test on any local spheres

xor ebx,ebx

.while ebx<.bodies.dCount

mov pBody,$OCall (.bodies::Collection.ItemAt, ebx)

.if eax!=pNode

OCall pBody::BBNode.Sweep_Node, pNode

.if eax==TRUE

.if edx==Colliding

;Spheres will Collide at a Future Time T0, and will part ways at Future Time T1.

mov edx,pRecords

mov eax,pBody

mov eax,.BBNode.pRigidBody

mov .CollisionPair.pBodyA,eax

mov eax,pNode

mov eax,.BBNode.pRigidBody

mov .CollisionPair.pBodyB,eax

fstp .CollisionPair.fLowerBound

fstp .CollisionPair.fUpperBound

add pRecords,sizeof CollisionPair

dec dMaxRecords

.elseif edx==Penetrating

;Spheres were already Penetrating at the start of the frame

;We will use default lower and upper limit values (0.0 and 1.0)

mov edx,pRecords

mov eax,pBody

mov eax,.BBNode.pRigidBody

mov .CollisionPair.pBodyA,eax

mov eax,pNode

mov eax,.BBNode.pRigidBody

mov .CollisionPair.pBodyB,eax

fldz

fstp .CollisionPair.fLowerBound

fld1

fstp .CollisionPair.fUpperBound

add pRecords,sizeof CollisionPair

dec dMaxRecords

.endif

.endif

.endif

inc ebx

.endw

;Traverse to the child suggested by the heuristic

.if .childcount!=0

mov edx,pNode

mov ebx,$OCall (esi.WhichChild, addr .BBNode.vPos,.BBNode.fRadius)

.if ebx >= 0 && .children!=0

mov edx,pNode

.if edx!=.children

mov esi,.children

jmp @B

.endif

.endif

.endif

.endif

MethodEnd

What's currently missing from this function is our prior knowledge of legal pairings in CollisionPair records (remember I made a fuss about this?) : the index of pNode must be greater than the index of pBody or else the test is a waste of time (since this body pairing must already have been queried !!)

That leaves us just a couple more methods to show.

Please note that the following methods are not a required part of the BBNode class, and specifically mention the RigidBody class (as our payload entities), I dislike this dependency and will likely change it.

The BBNode.Sweep_Node method will first obtain the relative velocity of two Spheres and make sure that they are in fact approaching one another.. it then performs a swept intersection test apon the two BoundingSpheres.

By convention, Body A is 'this' body (ie, the current node is a bounding-node) and Body B is 'other' body , as dictated by the given 'pOtherBodyNode' parameter (which is ALSO a bounding-node).

The swept test can return Clear, Colliding, Penetrating or Error.

Clear = these two Spheres will certainly not collide... the CollisionPair was a False Alarm!

Colliding = these two Spheres will first touch at time T0, and become separated again at time t1.

We call these two times 'the upper and lower bounds of the Time Of Impact".

These times are "Normalized" - ie, represent a fraction of the current timestep, from 0.0 to 1.0

Penetrating = these two Spheres were already in Penetration at the beginning of the timestep, so a conventional swept test won't work - however, given that these spheres are in fact moving apart from each other toward Separation, I've been thinking that we could perform a kind of 'reverse sweep' from 'the future' where they are separated, to determine our Upper and Lower bounds that way ???

Anyway, my current code simply sets the lower bound to 0.0, the upper bound to 1.0, and records a CollisionPair with these inaccurate bounds on the time of impact.

The final case of an Error I think can only happen when the two spheres are moving in parallel trajectories (thus our Quadratic Equation won't work)... think I can just return Clear for this one.

This brings us to the end of the Broadphase Collision code, just note that our FindCollisionPairs method generated a bunch of Queries which caused our tree to be updated - but we still need to call the UpdateNode method apon each bounding-node whose payload entity (RigidBody) has moved in space.

;** Returns values on FPU

;Calculate the Closing Velocity of two moving Bodies

;This is a floating point value whose Sign indicates whether the Bodies

;are moving toward each other (negative), or away from each other (positive)

;and whose scale indicates the magnitude of the relative velocity.

;Returns FOUR values on the FPU

;st(0) contains the Closing Velocity

;st(1-3) contains the DIRECTION of the Relative Velocity

Method BBNode.Get_Closing_And_Relative_Velocities,uses esi edi,pOtherBodyNode

LOCAL pOtherConfig

LOCAL vCollisionNormal_AB:Vec3

LOCAL vCollisionNormal_BA:Vec3

LOCAL dpa:Vec3, dpb:Vec3

LOCAL fMag:real8

SetObject esi

; Calculate 'change in position' of 'this' body

mov eax,.pRigidBody

fld .RigidBody.linPos.x ;current - old position

fsub .RigidBody.linPos0.x

fld .RigidBody.linPos.y

fsub .RigidBody.linPos0.y

fld .RigidBody.linPos.z

fsub .RigidBody.linPos0.z

fstp dpa.z

fstp dpa.y

fstp dpa.x

; Calculate 'change in position' of 'other' body

mov edx,pOtherBodyNode

mov eax,.BBNode.pRigidBody

fld .RigidBody.linPos.x ;current - old position

fsub .RigidBody.linPos0.x

fld .RigidBody.linPos.y

fsub .RigidBody.linPos0.y

fld .RigidBody.linPos.z

fsub .RigidBody.linPos0.z

fstp dpb.z

fstp dpb.y

fstp dpb.x

; Calculate RELATIVE VELOCITY vector, leave it on the fpu

lea eax,dpb

lea edx,dpa

fld .Vec3.x

fsub .Vec3.x

fld .Vec3.y

fsub .Vec3.y

fld .Vec3.z

fsub .Vec3.x

; Calculate Direction from A to B

mov edx,pOtherBodyNode

mov eax,.BBNode.pRigidBody

mov edx,.pRigidBody

fld .RigidBody.linPos.x ;current B - current A position

fsub .RigidBody.linPos.x

fld .RigidBody.linPos.y

fsub .RigidBody.linPos.y

fld .RigidBody.linPos.z

fsub .RigidBody.linPos.z

fstp vCollisionNormal_AB.z

fstp vCollisionNormal_AB.y

fstp vCollisionNormal_AB.x

;Calculate Magnitude for Normalizing the Direction

fld vCollisionNormal_AB.x

fmul st(0),st(0)

fld vCollisionNormal_AB.y

fmul st(0),st(0)

fld vCollisionNormal_AB.z

fmul st(0),st(0)

fadd

fadd

fsqrt

fstp fMag

;Normalize and Invert

fld vCollisionNormal_AB.x

fld vCollisionNormal_AB.y

fld vCollisionNormal_AB.z

fld1

fdiv fMag

fmul st(3),st(0)

fmul st(2),st(0)

fmul

fst vCollisionNormal_AB.z

fchs

fstp vCollisionNormal_BA.z

fst vCollisionNormal_AB.y

fchs

fstp vCollisionNormal_BA.y

fst vCollisionNormal_AB.x

fchs

fstp vCollisionNormal_BA.x

; Closing Velocity of two Bodies = (dpa . vCollisionNormal_AB) + (dpb . vCollisionNormal_BA)

invoke Vec3_Dot, addr dpa, addr vCollisionNormal_AB

invoke Vec3_Dot, addr dpb, addr vCollisionNormal_BA

fadd

MethodEnd

;This macro implements a classic "Quadratic Equation"

QuadraticFormula macro

; q = b*b - 4*a*c

fld _b

fmul st(0),st(0)

fld _a

fmul _c

fadd st(0),st(0)

fadd st(0),st(0)

fsub

fstReg eax

;if q >= 0

.ifBitSet eax, BIT31

fUnload

mov eax, FALSE ;complex root

.else

; sq = sqrt(q)

fsqrt

fstp sq

;d = 1 / (2*a)

fld1

fld _a

fadd st(0),st(0)

fdiv

fstp _d

;u0 = ( -b + sq ) * d

fld _b

; fchs

fadd sq

fmul _d

fstp u0

;u1 = ( -b - sq ) * d

fld _b

; fchs

fsub sq

fmul _d

fstp u1

;load fpu with T = smaller of two impact times

fMin u0, u1

mov eax,TRUE

.endif

endm

;Perform Broad-Phase collision testing of a pair of BoundingSpheres.

;Convention is that 'this body' is the potential 'aggressor', colliding with 'other body(s)'.

;Returns EAX = TRUE (success) or FALSE (failed)

; If eax=TRUE, EDX = Clear, Penetrating or Colliding

; If edx = Colliding, ST0 = start, ST1 = end time of interpenetration

;

Method BBNode.Sweep_Node,uses esi, pOtherBodyNode

LOCAL AB:Vec3 ;vector between origins

LOCAL rab2:real8 ;squared sum of radii

LOCAL _a, _b, _c, _d

LOCAL u0,u1 ;normalized time of first and second collisions

LOCAL sq

LOCAL ClosingVelocity:real4

LOCAL RelativeVelocity:Vec3

SetObject esi

;perform sphere/sphere sweep test...

;Get the Closing velocity of the Spheres, and also the Relative velocity vector

OCall esi.Get_Closing_And_Relative_Velocities, pOtherBodyNode

fstp ClosingVelocity ;= Signed Magnitude

fstp RelativeVelocity.z ;= Direction

fstp RelativeVelocity.y

fstp RelativeVelocity.x

;Make sure that the Spheres are actually approaching one another..

;If we find that they are actually moving apart (we are assuming they were not already penetrating)

;then collision isnt possible and we can quit early

.ifBitClr ClosingVelocity,BIT31

DbgWarning "Spheres are Separating"

mov eax,TRUE

mov edx,Clear

ExitMethod

.endif

;Calculate vector between starting points

; AB = previous pos B - previous pos A

mov eax,pOtherBodyNode

mov eax,.BBNode.pRigidBody

mov edx,.pRigidBody

fld .RigidBody.linPos0.x

fsub .RigidBody.linPos0.x

fld .RigidBody.linPos0.y

fsub .RigidBody.linPos0.y

fld .RigidBody.linPos0.z

fsub .RigidBody.linPos0.z

fstp AB.z

fstp AB.y

fstp AB.x

;Calculate sum of sphere radii, squared

; rab = ra + rb

fld .RigidBody.fRadius

fadd .RigidBody.fRadius

fmul st(0),st(0)

fstp rab2 ; = sum of sphere radii, squared

invoke Vec3_Dot, addr RelativeVelocity, addr RelativeVelocity

fstp _a ; = Relative Velocity, squared

invoke Vec3_Dot, addr RelativeVelocity, addr AB

fadd st(0),st(0)

fstp _b ; = Relative Velocity * Starting Distance * 2

invoke Vec3_Dot, addr AB, addr AB

fsub rab2

fstp _c ; = Starting Separation/Penetration Depth, squared

;check if spheres are (already) overlapping (at their starting positions)

.ifBitSet _c,BIT31

;The spheres are ALREADY intersecting in their START POSITIONS

;We're going to have to check the Bodies for Penetration.

DbgWarning "Spheres were already intersecting in the Previous Frame"

mov eax, TRUE

mov edx,Penetrating

.else

;check if spheres will collide anywhere along their travel paths

QuadraticFormula

.if eax==TRUE ;FPU contains ST0 = start, ST1 = end of sphere interpenetration

mov edx, Colliding ;Note: T is Normalized (its a time scalar)

.else

;They can't possibly collide ..

mov eax,TRUE

mov edx,Clear

.endif

.endif

DbgWarning "Sphere trajectories are parallel and so intersection is not possible"

mov eax,FALSE

MethodEnd

We have two kinds of numerical integration available to us: RK4 and Euler.

The RK4 integration is four times more expensive (but four ORDERS more accurate!).

Pseudocode for integrating using the RK4 integrator:

Our physics timestep begins by using RK4 to calculate the proposed state that each physical body will reach at the end of the current timestep, assuming no collisions occurred.

We then perform our broadphase test to create a (hopefully short) list of which pairs of BoundingSpheres will come into contact during the current timestep.

Then we perform semi-fine testing to determine which pairs of entites actually DO collide,and at what Future Time.

If no collisions occurred, we can advance our simulation time by one whole timestep... we are done.

Assuming there was one or more collisions, we need to unwind the simulation to the beginning of the timestep.. but we don't need to unwind ALL the bodies, just the ones involved in the earliest collision event. Now we'll integrate forward again - this time using the Euler integrator, andby a PARTIAL timestep, to the earliest collision time. we'll now resolve this collision, and then we'll perform a refresher broadphase test on all of the CollisionPairs which mention these two bodies - this may generate some new CollisionPairs based on the two bodies whose collision was resolved, and it may also kill some CollisionPairs which are no longer valid.

We will repeat this process as long as there is at least one CollisionPair with a TOI that falls within the current timestep - for those occuring further in the future than this, we note them, but we don't resolve them. Eventually we will have reached time T1, and have integrated our bodies to the end of the current timestep - any bodies which were not involved in collisions were integrated just ONCE, but may have been involved in multiple Swept tests.

What we DON'T want to be doing is winding the ENTIRE simulation back and forth while handling collisions - anything that makes it to the end of the timestep in a single RK4 pass, and which is not harrassed by any objects as a consequence of their collision response, will not need further processing.

Although we switch from using RK4 to using single-step Euler integration while resolving collisions, it should be noted that this will always involve something LESS than a full timestep.

To improve accuracy even further, we could determine which 25% of the timestep is involved, and use the physics state associated with the closest RK4 stage... in other words, we can consider the four stages of RK4 as being four euler substeps - four smaller linear integrations, with bounding times of 0-0.25, 0.25-0.5, 0.5,-0.75 and 0.75 to 1.0 (fractions of timestep). We've already calculated them all, we may as well be using them to our advantage, as the preferred bounds for our TOI search.

EG:

Intersections in the 1st Quarter can use linPos0 as the 'start time' during searches.

Intersections in the 2nd Quarter can use linPos1 as the 'start time' during searches.

Intersections in the 3rd Quarter can use linPos2 as the 'start time' during searches.

Intersections in the 4th Quarter can use linPos3 as the 'start time' during searches.

Intersections in the 1st Quarter can use linPos1 as the 'end time' during searches.

Intersections in the 2nd Quarter can use linPos2 as the 'end time' during searches.

Intersections in the 3rd Quarter can use linPos3 as the 'end time' during searches.

Intersections in the 4th Quarter can use linPos as the 'end time' during searches.

The RK4 integration is four times more expensive (but four ORDERS more accurate!).

Pseudocode for integrating using the RK4 integrator:

Call RKInit to backup current state and set up initial Forces (due to gravity and velocity)

Apply external forces due to Constraints and ForceGenerators

Call RKStep1

Apply external forces due to Constraints and ForceGenerators

Call RKStep2

Apply external forces due to Constraints and ForceGenerators

Call RKStep3

Apply external forces due to Constraints and ForceGenerators

Call RKStep4

Our physics timestep begins by using RK4 to calculate the proposed state that each physical body will reach at the end of the current timestep, assuming no collisions occurred.

We then perform our broadphase test to create a (hopefully short) list of which pairs of BoundingSpheres will come into contact during the current timestep.

Then we perform semi-fine testing to determine which pairs of entites actually DO collide,and at what Future Time.

If no collisions occurred, we can advance our simulation time by one whole timestep... we are done.

Assuming there was one or more collisions, we need to unwind the simulation to the beginning of the timestep.. but we don't need to unwind ALL the bodies, just the ones involved in the earliest collision event. Now we'll integrate forward again - this time using the Euler integrator, andby a PARTIAL timestep, to the earliest collision time. we'll now resolve this collision, and then we'll perform a refresher broadphase test on all of the CollisionPairs which mention these two bodies - this may generate some new CollisionPairs based on the two bodies whose collision was resolved, and it may also kill some CollisionPairs which are no longer valid.

We will repeat this process as long as there is at least one CollisionPair with a TOI that falls within the current timestep - for those occuring further in the future than this, we note them, but we don't resolve them. Eventually we will have reached time T1, and have integrated our bodies to the end of the current timestep - any bodies which were not involved in collisions were integrated just ONCE, but may have been involved in multiple Swept tests.

What we DON'T want to be doing is winding the ENTIRE simulation back and forth while handling collisions - anything that makes it to the end of the timestep in a single RK4 pass, and which is not harrassed by any objects as a consequence of their collision response, will not need further processing.

Although we switch from using RK4 to using single-step Euler integration while resolving collisions, it should be noted that this will always involve something LESS than a full timestep.

To improve accuracy even further, we could determine which 25% of the timestep is involved, and use the physics state associated with the closest RK4 stage... in other words, we can consider the four stages of RK4 as being four euler substeps - four smaller linear integrations, with bounding times of 0-0.25, 0.25-0.5, 0.5,-0.75 and 0.75 to 1.0 (fractions of timestep). We've already calculated them all, we may as well be using them to our advantage, as the preferred bounds for our TOI search.

EG:

Intersections in the 1st Quarter can use linPos0 as the 'start time' during searches.

Intersections in the 2nd Quarter can use linPos1 as the 'start time' during searches.

Intersections in the 3rd Quarter can use linPos2 as the 'start time' during searches.

Intersections in the 4th Quarter can use linPos3 as the 'start time' during searches.

Intersections in the 1st Quarter can use linPos1 as the 'end time' during searches.

Intersections in the 2nd Quarter can use linPos2 as the 'end time' during searches.

Intersections in the 3rd Quarter can use linPos3 as the 'end time' during searches.

Intersections in the 4th Quarter can use linPos as the 'end time' during searches.

I've just double-checked, and none of the physics engines that I've studied which use RK4 integration are taking advantage of the four substates they calculated when it comes to narrowphase collision detection.

Even the engines which implement a full Contact Manifold fail to do so, it's not just those which seek a single Deepest Penetration and then attempt correct for it.

In fact, most of them don't bother to cache the resulting states of all four RK substeps - I do.

Can anyone explain to me why they're ignoring this valuable state data, or offer any reason why it's not a good idea to use these four substates when attempting to find the Time Of Impact?

The reason that we can't use the (first three) RK States for collision detection is simply that they are being stored as 'Partial Derivatives' - they need to be Weighted to make sense. This only occurs in the fourth and final step, as we shall soon see.

I'm going to walk through the code for integrating a single RigidBody forwards in Time.

First we'll examine the Euler first-order equations, then we'll examine RK4.

The reason we're bothering to go over this old ground is because this new physics engine operates in DSpace, unlike my previous engines, so we need to build up our understanding of DSpace and how to transform between various coordinate systems.

I'm pretty sure we all know what WorldSpace is, so I won't both with that.

We will define BodySpace (or simply BSpace) as being the coordinate system in which a given RigidBody was declared. So for example, if we've loaded a RigidBody from some MESH file on disk, then without moving or rotating anything, that Mesh is already in BSpace.

For such an arbitrary body, there is no guarantee that the Center Of Mass will coincide with the Origin of the body, or that its weight (mass) will be distributed equally along its Principle Axes.

If we calculate the Inertia Tensor of some arbitrary body, we will get a 3x3 matrix which describes its mass-distribution in BodySpace.

Diagonalized BodySpace (or simply DSpace) is a coordinate system where the body's Center Of Mass DOES align with (0,0,0) ... and the body is oriented such that its weight is well-distributed. These are useful properties in dynamics equations as they allow us to use shorter forms of the math.

But the most outstanding property of DSpace is that the Inertia Tensor, once transformed into DSpace, contains all zeroes, except for the three 'diagonals', leaving us with effectively a Vec3.

That lets us perform cheap Vector math wherever we would normally need Matrix math... ie, any operations involving the Inertia Tensor can now be performed with a simple vector multiplication.

For these reasons, we choose to perform our dynamics calculations in DSpace rather than BSpace.

I won't show you yet how to calculate the Inertia Tensor, or Diagonalize it, or anything like that... for now we'll just assume that yes, we calculated the BSpace tensor, and transformed it into DSpace where it is represented by a Vec3.

Now we will simply begin looking at the core of our simulation, which is how we advance the simulation by integrating the physical state of each body over time.

Somewhere out there, Richard Chaney is having a laugh at me following his ten-year-old work.

I'm not laughing, his engine is being used in state-of-the-art commercial car racing sims, both by auto companies and by game developers. His work is illuminating and ahead of its time, unfortunately he didn't publish much in the way of educational material, just demos and source. There is one crappy PDF file about integrating single rigid bodies, and it doesn't even mention diagonalized space, which is the cornerstone of his work.

I'm going to walk through the code for integrating a single RigidBody forwards in Time.

First we'll examine the Euler first-order equations, then we'll examine RK4.

The reason we're bothering to go over this old ground is because this new physics engine operates in DSpace, unlike my previous engines, so we need to build up our understanding of DSpace and how to transform between various coordinate systems.

I'm pretty sure we all know what WorldSpace is, so I won't both with that.

We will define BodySpace (or simply BSpace) as being the coordinate system in which a given RigidBody was declared. So for example, if we've loaded a RigidBody from some MESH file on disk, then without moving or rotating anything, that Mesh is already in BSpace.

For such an arbitrary body, there is no guarantee that the Center Of Mass will coincide with the Origin of the body, or that its weight (mass) will be distributed equally along its Principle Axes.

If we calculate the Inertia Tensor of some arbitrary body, we will get a 3x3 matrix which describes its mass-distribution in BodySpace.

Diagonalized BodySpace (or simply DSpace) is a coordinate system where the body's Center Of Mass DOES align with (0,0,0) ... and the body is oriented such that its weight is well-distributed. These are useful properties in dynamics equations as they allow us to use shorter forms of the math.

But the most outstanding property of DSpace is that the Inertia Tensor, once transformed into DSpace, contains all zeroes, except for the three 'diagonals', leaving us with effectively a Vec3.

That lets us perform cheap Vector math wherever we would normally need Matrix math... ie, any operations involving the Inertia Tensor can now be performed with a simple vector multiplication.

For these reasons, we choose to perform our dynamics calculations in DSpace rather than BSpace.

I won't show you yet how to calculate the Inertia Tensor, or Diagonalize it, or anything like that... for now we'll just assume that yes, we calculated the BSpace tensor, and transformed it into DSpace where it is represented by a Vec3.

Now we will simply begin looking at the core of our simulation, which is how we advance the simulation by integrating the physical state of each body over time.

Somewhere out there, Richard Chaney is having a laugh at me following his ten-year-old work.

I'm not laughing, his engine is being used in state-of-the-art commercial car racing sims, both by auto companies and by game developers. His work is illuminating and ahead of its time, unfortunately he didn't publish much in the way of educational material, just demos and source. There is one crappy PDF file about integrating single rigid bodies, and it doesn't even mention diagonalized space, which is the cornerstone of his work.

Whether we're using Euler, RK4 or some other integration technique, we will always have to set up the Forces for each Body before stepping the system forwards in time.

There are two kinds of Forces that need to be accounted for.

Each body has some "internal forces" which are due to its Linear and Angular Momentum.

You may remember from school that Momentum is simply Mass * Velocity

In our math we will temporarily ignore Mass, and assume that Mass=1.0, ie, Unit Mass.

This allows us to directly use Velocity in place of Momentum in our equations (ie we can pretend we're doing Particle Dynamics with Unit Masses) - provided that we scale the result by the actual Mass involved, everything is peachy.

So, at the beginning of each TimeStep in our simulation, we will set our internal (linear and angular) Force to simply the (linear and angular) Velocity.

The other kind of force we must account for is "external forces".

The most obvious of these is Gravity - this is a force, it comes from outside of the body, and will affect its dynamics, so we need to add the Gravity force now. But external forces can also come from other sources - the most common will be the forces generated by Constraints such as Springs and other Joints.

Once we've calculated all the forces acting on a body, we're ready to integrate it forwards by some amount of Time, which will usually be our TimeStep, often it's 0.1 seconds per timestep, for a total of ten physics updates per second.

Euler's integration performs the following:

#1- Position is the first-order derivative of Velocity... that is to say, Velocity is the Rate Of Change Of Position Over Time. To update our Position, we need to calculate the Change Of Position due to Velocity and Time.

Position = Position + (Velocity * Time)

#2- Velocity is the first-order derivative of Acceleration... that is to say, Acceleration is the Rate Of Change Of Velocity Over Time. To update our Velocity, we need to calculate the Change Of Velocity due to Acceleration. For the Euler method, we won't use Acceleration directly...we'll use a Force-based variation ... We may recall that Force = Mass * Acceleration (F = ma) ... so if we know the Linear Force, and we know the Mass, we could calculate the Acceleration as simply "Acceleration = Force / Mass", and so then the Change Of Velocity would be "deltaVelocity = Acceleration * Time"

LinearVelocity = LinearVelocity + (LinearForce/Mass * Time)

#3- The Angular Position (ie Orientation), like its linear counterpart, must advance based on Velocity.

If we were storing our orientation using a Matrix (as I have in previous engines) then we could calculate a 'star matrix' representing a rotation of one Radian, scale it by the Angular Velocity, and then add the resulting matrix to the current orientation matrix. But this time we're using a Quaternion representation of orientation. We simply need to multiply our Quaternion by 0.5 * AngularVelocity * Time:

where RKh2 = TimeStep/2 ... watch the Sign in those operations!

Having done that, we will make sure that the resulting Quaternion is (close to being) Normal length, and then we'll extract a 3x3 rotation matrix from it - straight away I see 16 multiplications - I really need to make sure that I'm getting good value from using Quaternions for orientation , hmmz

#4- Now we have a new Position, Linear Velocity, and Orientation... it's time to update our Angular Velocity - how to do that? We can extract a new Angular Velocity from the Angular Force (aka Torque) and Time. I'll just show the equation here:

where RKh = TimeStep, and the diagBlah values are speedups based on our DSpace Inertia Tensor (vector) components - this will make sense when we look at the RigidBody.Init method :)

That's it - now we have a new Position, Orientation, Linear Velocity and Angular Velocity which our body will be in at the END OF THE TIMESTEP, ie, this is the new proposed physical state of our body at the end of the current timestep, assuming that there are no collisions we would accept this new state as our current state, and we're ready to make another timestep.

The equations used in the RK4 integrator are extremely similar to these ones, and we'll look at them in the next post, before digging into the nuts and bolts of that DSpace stuff.

What I really wanted you to notice is the ORDER of the operations, and how each derivative feeds into the next...

#1 - First we calculated Forces, both linear and angular, both internal and external.

#2 - LINEAR Then we updated Position according to Velocity and Time

#3 - LINEAR Then we updated Velocity according to Acceleration (via Force) and Time

#4 - ANGULAR Then we updated Orientation according to Angular Velocity and Time

#5 - ANGULAR Then we updated Angular Velocity according to Angular Acceleration (via Angular Force) and Time

If you note that the internal forces are due to the velocities, then you should see a closed loop of these five steps - we could imagine step 6 is to recalculate the forces, which is the same as step 1.

There are two kinds of Forces that need to be accounted for.

Each body has some "internal forces" which are due to its Linear and Angular Momentum.

You may remember from school that Momentum is simply Mass * Velocity

In our math we will temporarily ignore Mass, and assume that Mass=1.0, ie, Unit Mass.

This allows us to directly use Velocity in place of Momentum in our equations (ie we can pretend we're doing Particle Dynamics with Unit Masses) - provided that we scale the result by the actual Mass involved, everything is peachy.

So, at the beginning of each TimeStep in our simulation, we will set our internal (linear and angular) Force to simply the (linear and angular) Velocity.

The other kind of force we must account for is "external forces".

The most obvious of these is Gravity - this is a force, it comes from outside of the body, and will affect its dynamics, so we need to add the Gravity force now. But external forces can also come from other sources - the most common will be the forces generated by Constraints such as Springs and other Joints.

Once we've calculated all the forces acting on a body, we're ready to integrate it forwards by some amount of Time, which will usually be our TimeStep, often it's 0.1 seconds per timestep, for a total of ten physics updates per second.

Euler's integration performs the following:

#1- Position is the first-order derivative of Velocity... that is to say, Velocity is the Rate Of Change Of Position Over Time. To update our Position, we need to calculate the Change Of Position due to Velocity and Time.

Position = Position + (Velocity * Time)

#2- Velocity is the first-order derivative of Acceleration... that is to say, Acceleration is the Rate Of Change Of Velocity Over Time. To update our Velocity, we need to calculate the Change Of Velocity due to Acceleration. For the Euler method, we won't use Acceleration directly...we'll use a Force-based variation ... We may recall that Force = Mass * Acceleration (F = ma) ... so if we know the Linear Force, and we know the Mass, we could calculate the Acceleration as simply "Acceleration = Force / Mass", and so then the Change Of Velocity would be "deltaVelocity = Acceleration * Time"

LinearVelocity = LinearVelocity + (LinearForce/Mass * Time)

#3- The Angular Position (ie Orientation), like its linear counterpart, must advance based on Velocity.

If we were storing our orientation using a Matrix (as I have in previous engines) then we could calculate a 'star matrix' representing a rotation of one Radian, scale it by the Angular Velocity, and then add the resulting matrix to the current orientation matrix. But this time we're using a Quaternion representation of orientation. We simply need to multiply our Quaternion by 0.5 * AngularVelocity * Time:

;qRotPos0 = qRotPos

;qRotPos.w -= RKh2 * (qRotPos0.x * rotVel.x + qRotPos0.y * rotVel.y + qRotPos0.z * rotVel.z)

;qRotPos.x += RKh2 * (qRotPos0.w * rotVel.x - qRotPos0.z * rotVel.y + qRotPos0.y * rotVel.z)

;qRotPos.y += RKh2 * (qRotPos0.z * rotVel.x + qRotPos0.w * rotVel.y - qRotPos0.x * rotVel.z)

;qRotPos.z += RKh2 * (qRotPos0.x * rotVel.y - qRotPos0.y * rotVel.x + qRotPos0.w * rotVel.z)

where RKh2 = TimeStep/2 ... watch the Sign in those operations!

Having done that, we will make sure that the resulting Quaternion is (close to being) Normal length, and then we'll extract a 3x3 rotation matrix from it - straight away I see 16 multiplications - I really need to make sure that I'm getting good value from using Quaternions for orientation , hmmz

#4- Now we have a new Position, Linear Velocity, and Orientation... it's time to update our Angular Velocity - how to do that? We can extract a new Angular Velocity from the Angular Force (aka Torque) and Time. I'll just show the equation here:

; calculate new angular velocity with Eulers equations

; rotVel1.x = rotVel.x + (rotVel.y * rotVel.z * diagIyyMinusIzzOverIxx + rotForce.x * oneOverDiagIxx) * RKh

; rotVel1.y = rotVel.y + (rotVel.z * rotVel.x * diagIzzMinusIxxOverIyy + rotForce.y * oneOverDiagIyy) * RKh

; rotVel1.z = rotVel.z + (rotVel.x * rotVel.y * diagIxxMinusIyyOverIzz + rotForce.z * oneOverDiagIzz) * RKh

; rotVel = rotVel1

where RKh = TimeStep, and the diagBlah values are speedups based on our DSpace Inertia Tensor (vector) components - this will make sense when we look at the RigidBody.Init method :)

That's it - now we have a new Position, Orientation, Linear Velocity and Angular Velocity which our body will be in at the END OF THE TIMESTEP, ie, this is the new proposed physical state of our body at the end of the current timestep, assuming that there are no collisions we would accept this new state as our current state, and we're ready to make another timestep.

The equations used in the RK4 integrator are extremely similar to these ones, and we'll look at them in the next post, before digging into the nuts and bolts of that DSpace stuff.

What I really wanted you to notice is the ORDER of the operations, and how each derivative feeds into the next...

#1 - First we calculated Forces, both linear and angular, both internal and external.

#2 - LINEAR Then we updated Position according to Velocity and Time

#3 - LINEAR Then we updated Velocity according to Acceleration (via Force) and Time

#4 - ANGULAR Then we updated Orientation according to Angular Velocity and Time

#5 - ANGULAR Then we updated Angular Velocity according to Angular Acceleration (via Angular Force) and Time

If you note that the internal forces are due to the velocities, then you should see a closed loop of these five steps - we could imagine step 6 is to recalculate the forces, which is the same as step 1.

Here is a simplification of the steps involved in RK4 integration:

RKInit :

pos0 = pos;

RKStep1 :

acc1 = force * (h / mass / 2.0);

vel1 = vel;

pos += vel1 * (h / 2.0);

vel += acc1;

RKStep2 :

acc2 = force * (h / mass / 2.0);

vel2 = vel;

pos = pos0 + vel2 * (h / 2.0);

vel = vel1 + acc2;

RKStep3 :

acc3 = force * (h / mass);

vel3 = vel;

pos = pos0 + vel3 * h;

vel = vel1 + acc3;

RKStep4 :

acc4 = force * (h / mass / 2.0);

pos = pos0 + (vel1 + 2.0 * (vel2 + vel3) + vel) * (h / 6.0);

It's a bit more complicated because we have to deal with Angular stuff as well as this Linear stuff.

But essentially we can use the same logic.

Also, we must calculate the (combined internal and external) Forces just before each RK step.

RKInit :

pos0 = pos;

RKStep1 :

acc1 = force * (h / mass / 2.0);

vel1 = vel;

pos += vel1 * (h / 2.0);

vel += acc1;

RKStep2 :

acc2 = force * (h / mass / 2.0);

vel2 = vel;

pos = pos0 + vel2 * (h / 2.0);

vel = vel1 + acc2;

RKStep3 :

acc3 = force * (h / mass);

vel3 = vel;

pos = pos0 + vel3 * h;

vel = vel1 + acc3;

RKStep4 :

acc4 = force * (h / mass / 2.0);

pos = pos0 + (vel1 + 2.0 * (vel2 + vel3) + vel) * (h / 6.0);

It's a bit more complicated because we have to deal with Angular stuff as well as this Linear stuff.

But essentially we can use the same logic.

Also, we must calculate the (combined internal and external) Forces just before each RK step.