ASM Community

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 :)

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

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.

;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

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.

;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

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.

;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

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.

;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’t 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’ll 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

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:

;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

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

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?

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:

    ;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.