I've decided to write a tutorial document about physics simulation, ostensibly for game development, and with stability and speed in mind.
If you have questions, or spot any errors, feel free to reply and I will make appropriate changes, but do note that I will be regularly deleting any bug reports from this thread afterwards, just to keep the thread "tidy" as this thread is not intended to contain informal discussion except where it directly bears on the work at hand.
This text will be updated at my leisure.

With that said, here are my notes:





3D Simulation:
Coordinate Systems, Transformation Matrices, and Similarity Transformations

What's WORLD SPACE?
World Space is the fixed 3D space describing the entire world (universe), with its fixed set of axes and its fixed Origin.
The Origin of WorldSpace is simply (0,0,0) - that's the middle of our 'universe'.
World Space never changes, but things can move around in it.

OK, I knew that, What's BODY SPACE?
Body Space is the local coordinate system of some entity in our world.
In this Space, the Body is in its "Bind Pose", ie, unrotated and untranslated.
Its probably easiest to understand using an example.
Let's say we defined a 3D Cube.
Its Origin is declared at (0,0,0), and it has no rotation.
We can say that the cube is DECLARED IN BODYSPACE.
We might want to Translate (move) it somewhere in WorldSpace, and also maybe Rotate it.
In fact, lets assume that we want MANY cubes in our world, at various locations and orientations.
In order to achieve this, we could either declare each cube's worldspace vertices explicitly, or we could build (for each cube instance) a TRANSFORMATION MATRIX which positions and orients (transforms) our 'reference cube' from BodySpace into WorldSpace.
We simply apply this matrix to each vertex in our 'bodyspace declaration', and we now have the positions of those vertices in WorldSpace.
It's worth mentioning at this point that transforming values from one coordinate system to another does not change any mechanical quantity, its just a way of looking at the same data from a different point of view.
The numbers change, but only because we are seeing them in another frame of reference.
Transformation matrices move values between frames of reference.
From the point of view of a translated cube, it's located at 0,0,0 and the world origin is somewhere else :) 

And now for something completely different.

Let's say we wanted to test for the intersection of a Ray and our Cube.
The Ray has been given in WorldSpace.
The Cube has been declared in BodySpace, and has six Planes to test against, also declared in BodySpace.
Do you think we should transform each Plane into WorldSpace and test it against the Ray?
No, thats painfully slow, and not necessary.
We can transform the Ray from WorldSpace into the BodySpace of our Cube EVEN WHEN IT IS ORIENTED AND POSITIONED.
This is backwards :)
Once we transform the Ray from WorldSpace into BodySpace, we can test it against the UNROTATED planes :)
How would we do this, since our transform matrix only goes from bodyspace to worldspace !!?!
All we need to do is find the 'inverse' of our transform matrix
This makes a 'backwards matrix', which we can use to transform in the 'other direction'. Calculating the Inverse of a general matrix is rather costly, but if we know that the matrix contains ONLY rotational information, then we can apply the "Transpose" operator to find its inverse... just imagine that we are flipping the Axes, but not the Angles about those axes, you'll see that its true.
Transpose() is MUCH faster than Inverse(), but ONLY useful for (typically 3x3) Rotation Matrices. If we wanted to use this method to quickly invert a 4x4 Matrix that contains only rotation and position, we would transpose the rotation elements of the matrix, and flip the signs of the positional elements.
If it contains anything else, we have to do it the hard way (not covered here).

And now a tricky one.
Let's say we wanted some 'bee' entity to orbit around our Cube, regardless of its orientation or position.
We want to move into the (translated and rotated) bodyspace of our cube, then we want to move and rotate (relative to the cube) in order to position our 'bee', and then we want to know where that bee is located in WorldSpace.

Lets say the bee's transform (again, relative to cube bodyspace) is called B,
and that A = our Body-to-World matrix, and tA = our World-to-Body matrix

then:
C = A*B*Inverse(A)

We have calculated a third matrix C whose function is to spit out the location of our bee in WorldSpace.
Using C to transform our bee ONCE is far cheaper than transforming it several times, especially when we come to 'articulated bodies' such as 'Skinned Meshes', think about how you might find the end of your Big Toe in worldspace, you would have to transform your toe many times indeed.

In summary, a Similarity Transform is used to get us from Space A to Space B, perform some manipulative transform in Space B, and get us back to Space A, where we started from.

So now we know something about coordinate systems, transform matrices, and similarity transforms.

Now I'd like to turn this discussion toward physics simulation, and in particular, the simulation of Rigid Bodies in three dimensions. I will not make this a lesson in physics however, I will be concentrating on the application of matrices to differentiate and integrate the equations of motion.

We will begin by discussing the 'Inertia Tensor' in some detail, which will include the concept of Diagonalising a symmetric, orthogonal matrix (such as a tensor or general transform matrix), and a decription of the Primary Axes of Rotation.

What's a RIGID BODY?
It's a geometric entity (such as a geometric primitive, a mesh, or even a random cloud of points) whose geometry never changes (although it may be free to rotate, it is never deformed), and which has MASS (due to the Density of the material from which it is formed - I will not explain simple concepts of physics in this article, although I will mention them, its up to YOU to find out what those are all about).

What's an INERTIA TENSOR?
Also known as "Mass Moment of Inertia Tensor", or simply "MMOI tensor".
It's a matrix which describes the distribution of mass of a Body.
For some common geometries, this matrix is Known, we just plug in the appropriate values.
But we can calculate an Inertia Tensor for any Rigid Body, ie, ANY ARBITRARY GEOMETRY.
I'll explain how to do that immediately, and then explain how MMOI tensors are useful to us.

To calculate the tensor, we NEED to know the Mass of the Body, and we may need to calculate the Center of Mass (COM) of the Body, which is NOT NECESSARILY the same as its Origin, OR its 'Centroid'. Calculation of the COM is based on the concept that our Body is comprised of "Particles of Mass", ie, we attribute some Mass to each Vertex in our Body (be patient, I'll get to that too).
For now, let's assume we KNOW what our Body weighs. We know the Mass. But we don't know the COM.

Here's some sourcecode that calculates the COM by integrating over the Volume of the Body.
I won't explain how it works, investigate COM further in your own time, I have too much to cover !!


float3 CenterOfMass(const float3 *vertices, const int3 *tris, const int count)
{
// count is the number of triangles (tris)
float3 com(0,0,0);
float  volume=0; // actually accumulates the volume*6
for(int i=0; i < count; i++)  // for each triangle
{
float3x3  A(vertices [ tris [0] ],
                                      vertices [ tris [1] ],
                                      vertices [ tris [2] ]); 
float vol=Determinant(A);  // dont bother to divide by 6
com += vol * (A.x+A.y+A.z);  // divide by 4 at end
volume+=vol;
}
com /= volume*4.0f;
return com;
}


Let's get back on track...
The MMOI tensor is defined as:
+Ixx -Ixy -Ixz
-Ixy +Iyy -Iyz
-Ixz -Iyz +Izz

Note that some of the values in this matrix are repeated - in fact, its a Symmetrical matrix, because if we apply the Transpose operator to it (swap the Diagonals), the matrix is not changed at all.

The elements of the tensor are calculated as follows:
m = "sum of particle"
Ixx = m.y^2+m.z^2 (ie, sum of all particle ysquared + zsquared)
Iyy = m.x^2+m.z^2
Izz = m.x^2+m.y^2
Ixy = m.x * m.y
Ixz = m.x * m.z
Iyz = m.y * m.z

Armed with that information, we can now make a MMOI tensor for any shape, but lets take a look at some of the "Known" tensors for Primitives. Note that they are all "diagonal" matrices :)

For a SOLID Sphere, the tensor is:
(2/5m.x*R^2) 0 0
0 (2/5m.y*R^2) 0
0 0 (2/5m.z*R^2)

For a HOLLOW Sphere, the tensor is:
(2/3m.x*R^2) 0 0
0 (2/3m.y*R^2) 0
0 0 (2/3m.z*R^2)

For a SOLID Box, the tensor is:
(m.y^2+m.z^2)/12 0 0
0 (m.x^2+m.z^2)/12 0
0 0 (m.x^2+m.y^2)/12

There are more 'Known' tensors for simple shapes, but the thing to notice about all of the above shapes is that the tensor is very much like an Identity matrix, except that we have some Scaled values in the diagonals instead of simply having 1.0 in there. These are all examples of "diagonalised matrices".
For more complex shapes, the tensor we calculate will not always be diagonal.
The statements I am about to make are NOT meant to be obvious to the reader:
A diagonal tensor implies that the mass of the Body is evenly distributed about the axes... and for ANY arbitrary Body, there exists some orientation where this is TRUE.
If the tensor we calculated is not Diagonal, we can make it so (see Jacobian Rotation and Gauss-Jordan Elimination for two possible solutions), and in doing so, we can calculate a Matrix that gets us from BodySpace into "Diagonalised BodySpace", and we can then Transpose it to find its counterpart, which moves us from DSpace back to BSpace. Furthermore, according to the Similarity Transform technique, we can find matrices that will get us from WorldSpace into DSpace, and as usual, we can Transpose it.

Why is it NICE to work in the Diagonalised BodySpace? Why can't we just use a "messy" tensor?
We CAN, but its far slower than working in DSpace, because when we want to multiply the tensor with something else, we can treat the Diagonal (non-zero) elements as a Vector3,  and ignore all the Zeros, a huge speedup!!
So, DSpace is really some Orientation of our BodySpace axes, ie, its a BSpace coordinate system that has been rotated about the Body's COM.
Are we comfortable with the concept of working in "another spatial coordinate system" other than WorldSpace?
We should be by now!

Now we know what Diagonalised BodySpace is, and why it's nice, and what a Diagonal matrix is, and what a Diagonalised MMOI tensor is, but we still haven't explained WHY we need a tensor at all!
What is the Inertia Tensor GOOD FOR?

Well, if we apply a Force to our Body, depending on where we apply it relative to the COM, it will try to move and/or rotate... the Inertia Tensor can be used to calculate how input forces affect the linear and angular momentum, which in turn cause this motion and rotation stuff to happen.... it can be used to calculate exactly how the body behaves when we poke it with a stick :)

Ok, it might not be a stick, the forces can come from rockets, punches, explosions, motors, springs, gravity, whatever... forces cause changes in momentum, and the tensor is a tool to calculate that change.

Now that we can calculate changes in (linear and angular, aka 'total') momentum, we know pretty much everything we need to know in order to simulate rigid bodies which conserve "total momentum" in response to input forces , ie, which 'conserve energy' (one of the Laws of Physics).

I won't cover collision detection and response in this article, suffice to say that it involves the calculation of collision forces at the moment of impact.


You're still here? Good !! That was a heck of a lot to take in, wasn't it?
I'm sure it's already got you thinking :)

A note about stability: FIX YOUR TIMESTEP.
It's so important, it deserves its own discussion !!
One of the WORST things you can do is simply hand "fElapsedTime" to your physics code. This is not obvious, but the result of "jitter" in the elapsed time is magnified and accumulated in the physical state of your entities. Just don't do it.
How do we fix our timestep in such a way that our simulation runs at the same speed on all machines? We use a Time Accumulator, in much the same way as we might measure the rendering framerate.

Let's say that we've decided to apply a fixed timestep of 0.100 seconds.
1. At the start of the simulation, we set fAccumulatedTime to zero.
2. Inside our Rendering loop, we add elapsed time to the accumulator.
3. Now we check if the accumulated time is greater than the Fixed TimeStep.
If it is, we subtract one TimeStep from the accumulator, and repeat Step 3.
This means that on slower machines we might be performing several physics updates for each rendered Frame, and the fps might suffer.. I won't talk about this now.
But if the accumulated time is LESS than a TimeStep (common on fast machines), then we are "not yet ready to update the physics" - and have "spare time". It might be tempting to just render another Frame, but there's really not much point doing that since our entities haven't been updated, so we might think of other ways to use up this "spare time" that we have detected.
4. Since we have updated our Entities, we should now Render them all.

Now our TimeStep is fixed, and still dependant on elapsed time :)
I cannot stress how important this is to creating a STABLE simulation, and we really don't want stuff flying off into infinity (or vibrating annoyingly) if we can avoid it, do we?

It's almost time to start introducing some real code, so let's put together a quick list of major milestones that we're going to need to reach on our way to coolness:

1. Mass
For starters, we can just plug in some arbitrary Mass value for each entity, but later we might want to calculate it from Density and Volume.

2. Calculate Inertia Tensor (in BodySpace)
For starters, we can just plug in some Known tensor for each entity, but later we might want to calculate it from Mass and Geometry.

3. Calculate Inverse of the Inertia Tensor (in WorldSpace)
This is MUCH easier if we have a Diagonal tensor to begin with.
We have talked of WorldSpace, BodySpace and DiagonalisedSpace.
Diagonalising a Body does two things : it shifts the origin of the Body to coincide with the Center of Mass of the Body, and it rotates the Body such that the Principle Axes of the Body coincide with the World Axes.

Ro is the (initial) bodyspace-to-worldspace rotation transform matrix for a given Body..
rotPos and linPos represent the rotation and translation in WSpace.
oRotPos and oLinPos represent the rotation and translation in BSpace.
diagRotPos and diagLinPos represent the rotation and translation in DSpace.

The transform from BSpace to WSpace is :
(A) Rw = oRotPos * Ro + oLinPos
The transform from BSpace to DSpace is :
      Rd = diagRotPos * Ro + diagLinPos
The transform from DSpace to BSpace is:
(B) Ro =  transpose(diagRotPos) * (Rd - diagLinPos)
Finally, the transform from DSpace to WSpace is:
(C) Rw = rotPos * Rd + linPos
Now substituting (B) in (A) :
      Rw = oRotPos * trans(diagRotPos) * (Rd - diagLinPos) + oLinPos
(D) Rw = oRotPos * trans(diagRotPos) * (Rd - oRotPos * trans(diagRotPos)) * diagLinPos + oLinPos

Compare (D) with (C) , we can see how to directly obtain World-relative rotation, position and velocity...
      rotPos = oRotPos * trans(diagRotPos)
      linPos = oLinPos - oRotPos * trans(diagRotPos) * diagLinPos
      rotVel = diagRotPos * oRotVel

To calculate Initial linear Velocity (linVel) :
      COM is Rd = 0, so from (B) :
      COM_Ro = trans(diagRotPos) * (-diagLinPos)
      velocity of COM (BSpace) = oRotVel x COM_Ro
      velocity of COM (WSpace) =  oRotPos * (oRotVel x COM_Ro) + oLinVel

Enough of those formula, you're giving me a headache, and it makes me want to stop reading this right now!! If thats true, take a break, you've earned it.






We're ready to start writing code!
I know, there's MOUNTAINS of stuff I have yet to cover, and so far, I've really thrown you in at the deep end, forcing you to sink or swim. Unfair of me? Well just think, we've covered all the HARD stuff, so now it's all downhill from here - it can only get easier. Other texts present the information from easiest to hardest, so you start off readin and think "gee, this is all simple stuff that anyone can do" , and by the time you get to the end, you're struggling to keep your head above water, panicking, and wondering what the hell you have gotten yourself into, and whether you can really do it. By presenting the hard stuff up front, anyone who is STILL reading has a fair idea of what to expect. I think that's more fair on the reader than giving them an inferiority complex apon reading the last page in the book :)

GET CODING!
I will present all code using ObjAsm32's oopasm syntax.
If you don't like it, stiff cheese - this text is aimed at that audience, you can translate it.
Let's start with the entrypoiny to our Simulator, called from the main application loop.


Object Simulator, SimID, Primer
StaticMethod Simulate, real4
StaticMethod Update
DefineVariable fTimeAccumulator, real4, 0.0f
DefineVariable fTimeStep, real4, 0.100f
ObjectEnd

Method Simulator.Simulate, uses esi, fElapsedTime:real4
SetObject esi
;Accumulate the elapsed time
fld fElapsedTime
fadd .fTimeAccumulator
fstp .fTimeAccumulator
;Check if its time to update physics yet
@@: fMin .fTimeAccumulator, .fTimeStep
fstpReg eax
.if eax==.fTimeStep
;Yes, its time to update our physics
OCall Simulator.Update
;Subtract one physics timestep from the accumulator,
;then jump back to see if we require further updates
fld  .fTimeAccumulator
fsub .fTimeStep
fstp  .fTimeAccumulator
jmp @B
.endif
MethodEnd


It's a good start, yes?
Next, we will write our Update method.
We'll start with something rather basic, and extend apon it later.

Posted on 2008-01-24 01:47:24 by Homer
We're on a roll, so let's maintain our momentum, shall we?

Here I shall present the Update method, which is called by Simulate(), and is responsible for advancing the physics state of our closed system (universe) by one full TimeStep.

If we evolve the system forward in time and find we have a collision to deal with, we then Half the Timestep and try again, bisecting the timestep until we find a "happy place" BEFORE the intersection occurred, then we bisect the timestep AGAIN,  and continue moving forwards in time, repeating this until the penetration depth is below a given threshold. By doing this, we have implemented a "binary search in time" to find the moment when the entities made contact (ie the "kissing moment").
When we have found it, we then resolve the collision, apply the new forces, and continue to the end of the timestep.
You will notice that I will be using "Runge-Kutta Integration" aka RK4.
It's four times slower than Euler Integration that you learned at school, but its also 4 times more accurate.
This may be the only place that you'll see me choose accuracy over speed, but since integration is so important to simulating physics, I think its worth doing well.


Method Simulator.Update,uses esi,DeltaTime:real4
local CurrentTime,TargetTime

SetObject esi
    m2m TargetTime , DeltaTime ;Set up the "time until end of complete Step"
mov CurrentTime,0
    .repeat ;.while CurrentTime < DeltaTime

   
    fMin CurrentTime, DeltaTime
    fstpReg eax
    .break .if eax==DeltaTime
   
    ;Advance the State by one FULL RK Step
    OCall RKInit ;set up for RK SUBSTEP
    OCall ComputeForces ;find linForce and rotForce (from linPos, linVel, rotPos, rotVel.)
    OCall RKStep123, 1 ;perform the SUBSTEP

    OCall RKInit
    OCall ComputeForces    
    OCall RKStep123, 2

    OCall RKInit
    OCall ComputeForces    
    OCall RKStep123, 3

    OCall RKInit
      OCall ComputeForces    
    OCall RKStep4
   
   
    ;Check for and resolve any collisions
OCall CheckForCollisions
.if eax == Penetrating
; we simulated too far, so SUBDIVIDE TIMESTEP and try again..
; By doing this, we have implemented a Binary Search in Time
; for the 'moment of contact' when the collision will occur.
; IE, we are looking for the moment of "KISSING CONTACT"
; In theory, its possible to calculate that Time,
; and advance the system directly to that Time, which would
; eliminate this binary-search approach, but I'll leave that
; for a possible future reworking of this code.
            fld TargetTime
            fmul r4_Half
            fstp TargetTime

            ; blow up if we aren't moving forward each step, which is
            ; probably caused by interpenetration at the frame start
            fMin DeltaTime, fEpsilon
            fstpReg eax
            .if eax==DeltaTime
            DbgWarning "Error - failed to find non-penetrating state"
            DbgWarning "Physics Simulation Terminated!"           
            int 3
            .endif

        .else ;either Touching or Clear
       
        ;Any (Touching) collision has already been Resolved.
        ;We should now check the Kinetic Energy of this body..
        ;If its close to zero, we can flag the body as being 'at rest'.
        ;Bodies that are Resting are not tested as "collision aggressors"
        ;but can still be involved in collisions with MOVING bodies.
;...
;...

; We've made a successful (sub)step, so update our timecounter(s)
            m2m CurrentTime , TargetTime           
            m2m TargetTime , DeltaTime
        .endif
    .until 0
    ;We have moved the simulation ahead by one full timestep - yay!!!
   
MethodEnd



The idea is that CheckForCollisions() performs tests between pairs of Bodies, and terminates early if a collision OR touching state is found.
The collision is resolved immediately, forces calculated and applied, and the simulation continues.
This does not work for cases of multiple contact, I will give an example of how this affects our simulator, and then we will make the necessary changes to support multiple simultaneous contacts properly.

Imagine that we have a pool table, and in the middle of the pool table is one ball, at rest (Ball A)
Two humans stand at each end of the table, and each has a pool ball placed on the table in front of them (Balls B and C).
Let's suppose that they each launch their ball toward Ball A, and furthermore that their balls both make contact with Ball A simultaneously.
In the real world, the result is that energy is transferred THROUGH Ball A, between Balls B and C, which then bounce away, leaving Ball B at rest.
Our simulator tests PAIRS of entities, and resolves collisions immediately.
If we tried doing this experiment with our simulator, Ball B would collide with Ball A, and they'd both start moving... then Ball C would hit Ball A, and Ball A would be reflected back toward Ball B - it would NOT BE AT REST, and the simulation would not be correct.
It's just an example, but multiple contact resolution is desirable - and we can do it cheaply.

The solution is to add a "force accumulator" to each Body.
During collision detection, contact forces are calculated but NOT applied immediately.
Our collision detector does NOT terminate when the first collision is found.
Instead, it checks ALL pairs of entities, accumulates forces for each entity involved in a collision, and then when it has completed, the accumulated force of each entity is applied, and the simulation can then continue.

With that said, let's do a little work on the RigidBody class itself, and then we'll get back to the simulator.
We'll start with the guts, filling in the blanks as we go, which seems to be a theme among many programmers, and indeed, designers in general.

Note that I am inheriting the D3D_Mesh class, but I will nonetheless be redefining most of the methods in that class.


Object RigidBody, RigidBodyID, D3D_Mesh
; Runge-Kutta integration stuff - DONT SCREW WITH THESE
DefineVariable linPos0,Vec3,{<>}
DefineVariable linVel0,Vec3,{<>}
DefineVariable rotVel0,Vec3,{<>}
DefineVariable qRotPos0,Quaternion,{<>}
DefineVariable rotPos0,Mat33,{<>}
DefineVariable linVel1,Vec3,{<>}
DefineVariable rotVel1,Vec3,{<>}
DefineVariable linAcc1,Vec3,{<>}
DefineVariable rotAcc1,Vec3,{<>}
DefineVariable linVel2,Vec3,{<>}
DefineVariable rotVel2,Vec3,{<>}
DefineVariable linAcc2,Vec3,{<>}
DefineVariable rotAcc2,Vec3,{<>}
DefineVariable linVel3,Vec3,{<>}
DefineVariable rotVel3,Vec3,{<>}
DefineVariable linAcc3,Vec3,{<>}
DefineVariable rotAcc3,Vec3,{<>}

; Constant properties of body
DefineVariable mass,real4
DefineVariable oneOverMass,real4
DefineVariable minusMG,real4   ; -mass * g
DefineVariable cOfM,Vec3,{<>} ; centre of mass (in BSpace coords)
DefineVariable Ix,real4       ; Ix = sum of mx
DefineVariable Iy,real4
DefineVariable Iz,real4
DefineVariable Ixx,real4 ; Ixx = sum of m(y^2 + z^2)
DefineVariable Iyy,real4
DefineVariable Izz,real4
DefineVariable Ixy,real4 ; Ixy = sum of mxy
DefineVariable Iyz,real4 ; these are in BSpace coords
DefineVariable Izx,real4

; DSpace stuff
DefineVariable diagRotPos,Mat33,{<>} ; transform from BSpace to DSpace
DefineVariable diagLinPos,Vec3,{<>} ; Rd = diagRotPos * Rb + diagLinPos
DefineVariable diagIxx,real4 ; transformed moment of inertia (DSpace)
DefineVariable diagIyy,real4 ; also 1 / I * h for Euler integrater
DefineVariable diagIzz,real4
DefineVariable oneOverDiagIxx,real4
DefineVariable oneOverDiagIyy,real4
DefineVariable oneOverDiagIzz,real4
DefineVariable diagIyyMinusIzzOverIxx,real4
DefineVariable diagIzzMinusIxxOverIyy,real4
DefineVariable diagIxxMinusIyyOverIzz,real4

; damping
DefineVariable linKD,  real4                ; linear damping (-ve and multiplied by mass)
DefineVariable rotKDx, real4 ; angular damping (-ve and multiplied by Ixx, etc..)
DefineVariable rotKDy, real4
DefineVariable rotKDz, real4                 

; current state of body
DefineVariable linPos,Vec3,{<>} ;linear position and velocity
DefineVariable linVel,Vec3,{<>} ;of centre of mass (Rd = 0). (WorldSpace)

DefineVariable rotPos,Mat33,{<>} ; rotational position.
; transform from DSpace to WorldSpace is:
; Rw = rotPos * Rd + linPos

DefineVariable qRotPos,Quaternion,{<>} ; quaternion of rotPos matrix (matrix and quaternion are kept the same)
DefineVariable rotVel,Vec3,{<>}        ; rotational velocity (DSpace)

; current forces on body
DefineVariable linForce,Vec3,{<>}      ;<- this is our Force Accumulator :)
DefineVariable rotForce,Vec3,{<>}

;Tags and Bools and other stuff I ADDED
DefineVariable dBodyType,dword,NULL
DefineVariable bHasKineticEnergy,BOOL,FALSE ;TRUE = moving, FALSE = resting
;Only valid for Boxes and Clouds
DefineVariable NumberOfBoundingVertices,dword,NULL
DefineVariable pvBoundingVertices,Pointer,NULL
DefineVariable CoefficientOfRestitution,real4,1.0f
ObjectEnd


That might seem like a lot, and it is.
It's a LOT more than you'd find in , say, a particle simulator.
And I still haven't listed any Methods!
But many of those data fields are only needed when we are using non-primitive bodies, ie, when we need to diagonalise our inertia tensor because its not already diagonal (note : geometric primitives are).
And I won't be talking about diagonalising until later.
So you can effectively ignore many of them.
You may be wondering why there's four copies of some fields.
These are needed to pass information statefully between RK integration steps.
You might also want to know about that Quaternion / Matrix deal.
We will be converting our rotation matrix into a quat mainly to improve the stability by using it to constrain the matrix to "orthonormality" (orthogonal and normal). Also, we can find places where we can use the resulting quat to speed up some calculations.

Here's the list of Methods for RigidBody:

RedefineMethod Init, Pointer,Pointer,PointerLPVEC3,LPVEC3,LPQUAT,LPVEC3,real4,real4 ;(ptr_oLinPos, ptr_oLinVel, ptr_oQRotPos,  oRotVel,  linKD, rotKD)
     

; functions to set up rigid body mass distribution
StaticMethod clear
StaticMethod setBox,LPVEC3,real4 ;XYZ radius, Mass
StaticMethod setCylinder,real4,real4,real4 ;(radius, height, mass);
StaticMethod setSphere,real4,real4 ;(radius, mass);
StaticMethod combine,Pointer,LPMAT33,LPVEC3 ;(ptr_otherbody, ptr_rotPos, ptr_linPos);

; functions for initialising rigid body etc
StaticMethod InitMesh, Pointer, Pointer, Pointer  ;pOwner, pD3D, pTextureManager
StaticMethod diagonalise
StaticMethod setStepSize,real4

; functions to update from one frame to another
StaticMethod RKInit
StaticMethod eulerStep
StaticMethod RKStep123, dword
StaticMethod RKStep4
StaticMethod undo

functions to add force and torque
StaticMethod addBodyTorque,LPVEC3 ;(Vector3 & torque);
StaticMethod addWorldTorque,LPVEC3 ;(Vector3 & torque);
StaticMethod addWorldWorldForce,LPVEC3,LPVEC3 ;(Vector3 & force, Vector3 & pos);
StaticMethod addWorldBodyForce,LPVEC3,LPVEC3 ;(Vector3 & force, Vector3 & pos);
StaticMethod addBodyBodyForce,LPVEC3,LPVEC3 ;(Vector3 & force, Vector3 & pos);

; functions to get current position and velocity of points
StaticMethod findWorldPos,LPVEC3,LPVEC3 ;(Vector3 & bodyPos, Vector3 & worldPos);
StaticMethod findBodyPos,LPVEC3,LPVEC3 ;(Vector3 & worldPos, Vector3 & bodyPos);
StaticMethod findBodyWorldVel,LPVEC3,LPVEC3 ;(Vector3 & bodyPos, Vector3 & worldVel);
StaticMethod findWorldWorldVel,LPVEC3,LPVEC3 ;(Vector3 & worldPos, Vector3 & worldVel);
StaticMethod findWorldPosVel,LPVEC3,LPVEC3,LPVEC3;(Vector3 & bodyPos, Vector3 & worldPos, Vector3 & worldVel);

; functions for getting energy and momentum (for testing etc)
StaticMethod energy
StaticMethod momentum,LPVEC3,LPVEC3 ;(Vector3 & lin, Vector3 & rot);


Whoa!! That's a fair whack of code we can expect in this class!!
Some of these methods are utilitiarian and you won't need them.
But most are required by our simulator.

Next we'll start to look at the code for these methods, I'll be slapping the necessary physics information into the text on a need-to-know basis.
The main thing to note here is that before a body can be simulated, it needs to be Initialized.
We need to set up the initial physical state of the body.
For that, we have the Init method, and that's where I'll begin.
Posted on 2008-01-24 02:22:57 by Homer
The Init method requires some discussion.

The input params..
poLinPos, poLinVel  = ptrs to initial position and velocity (WSpace)
poQRotPos            = ptr to initial rotation quat (WSpace...will automatically be normalized)
poRotVel              = ptr to initial angular velocity (BSpace)
linKD, rotKD          = damping constants. will get scaled by the mass.

Initially, we should set all Matrices and Quaternions to "Identity" values.
Then we either call InitMesh to initialize the ancestpr D3D_Mesh object and calculate the inertia tensor, or alternatively,we can call one of the setGeometryShape (eg setBox) methods to set up our inertia tensor.
Then we call the Init method to set up the Mass properties.
Note that Init() will call diagonalise() even if the tensor is already diagonal.
This can be improved on.

;-------------------------------------------------------------------------------------

Method RigidBody.Init,uses esi, poLinPos:LPVEC3, poLinVel:LPVEC3, poQRotPos:LPQUAT, poRotVel:LPVEC3,linKD:real4, rotKD:real4
LOCAL oRotPos:Mat33
LOCAL M1:Mat33
LOCAL t1:Vec3
LOCAL COM:Vec3 ; centre of mass in old body coords
    SetObject esi

    ;Set up 1/Mass
    .if .fMass!=0
      fld1
      fld .mass
      fdiv
      fstp .oneOverMass
    .else
      mov .oneOverMass,0
    .endif   

    ;find matrix of initial rotation (convert quaternion to mat33)
    ;oRotPos = fromQuatL2(oQRotPos, mod2(oQRotPos))
    mov edx,poQRotPos
    Quaternion_mod2 .Quaternion
    fstpReg eax
    invoke Mat33_fromQuatL2, addr oRotPos, poQRotPos,eax
   
    ;Diagonalize the rotation matrix (using Jacobian rotation method):
    ;If we can 'magically' make all the non-diagonal (products of inertia) elements of
    ;our intertia tensor matrix become zero, leaving only the diagonals,
    ;we have found an orientation where the mass is evenly distributed around the axes...
;The non-zero diagonals are called the principal moments of inertia of the object.   
    OCall diagonalise
   
    ;You know what a TRANSPOSED matrix is, and what its use is?
    ;Lets transpose our Diagonalised inertia tensor: M1 = transpose(diagRotPos)
    invoke RtlMoveMemory,addr M1, addr .diagRotPos, sizeof Mat33
    Mat33_transpose M1

;You know what a SIMILARITY TRANSFORM is, and what its use is?
;Prepare a SIMILARITY TRANFORM MATRIX: rotPos = oRotPos * M1
    invoke Mat33_multiply,addr .rotPos,addr oRotPos, addr M1
   
    ;Convert M1 into Quaternion form
    ;qRotPos = fromMatrix(rotPos)
    invoke Quaternion_fromMatrix, addr .qRotPos, addr .rotPos

    ;t1 = rotPos * diagLinPos
    Vec3_mult_Mat33 .rotPos, .diagLinPos
    fstp t1.z
    fstp t1.y
    fstp t1.x
   
    ;linPos = oLinPos - t1
    mov edx,poLinPos
    fld .Vec3.x
    fsub t1.x
    fstp .linPos.x
    fld .Vec3.y
    fsub t1.y
    fstp .linPos.y
    fld .Vec3.z
    fsub t1.z
    fstp .linPos.z
   
    ;rotVel = diagRotPos * oRotVel
    mov edx,poRotVel   
    Vec3_mult_Mat33 .diagRotPos, .Vec3
    fstp .rotVel.z
    fstp .rotVel.y 
    fstp .rotVel.x

    ;COM = transpose(diagRotPos) * (-diagLinPos)
    Vec3_transMult .diagRotPos, .diagLinPos
    fchs
    fstp COM.z
    fchs
    fstp COM.y
    fchs
    fstp COM.x
   
    ;t1 = oRotVel x COM_Ro (note : crossproduct)
    mov edx,poRotVel
    CrossProduct .Vec3, COM
    fstp t1.z
    fstp t1.y
    fstp t1.x
   
    ;velocity of COM (world coords)
    ;linVel = (oRotPos * t1) + oLinVel                     
    Vec3_mult_Mat33 oRotPos, t1
    mov edx,poLinVel
    fadd .Vec3.z
    fstp .linVel.z
    fadd .Vec3.y
    fstp .linVel.y
    fadd .Vec3.x
    fstp .linVel.x

    ;scale damping by mass
    ;this->linKD  = -linKD / oneOvermass               
    ;this->rotKDx = -rotKD * diagIxx
    ;this->rotKDy = -rotKD * diagIyy
    ;this->rotKDz = -rotKD * diagIzz
    fld linKD
    fchs
    fdiv.oneOverMass
    fstp .linKD
    fld rotKD
    fchs
    fmul .diagIxx
    fstp .rotKDx
    fld rotKD
    fchs
    fmul .diagIyy
    fstp .rotKDy
    fld rotKD
    fchs
    fmul .diagIzz
    fstp .rotKDz
   
MethodEnd


RigidBody class can handle various kinds of primitive geometry, as well as complex geometries (meshes).
Furthermore, it can Combine the attributes of several Bodies (of any kind) into a single composite body!!
Let's look at one of the geometry selection methods.
Here's one of our Known tensors being initialized :)

Method RigidBody.setSphere,uses esi,radius:real4,mass:real4
    SetObject esi
    m2m .mass,mass
    xor edx,edx
    mov .cOfM.x,edx
    mov .cOfM.y,edx
    mov .cOfM.z,edx
    mov .Ix,edx
    mov .Iy,edx
    mov .Iz,edx   
    mov .Ixy,edx
    mov .Iyz,edx
    mov .Izx,edx 
    ;Ixx = Iyy = Izz = mass * radius * radius * 2.0 / 5.0;
    fld radius
    fmul st(0),st(0)
    fmul mass
    fadd st(0),st(0)
    fdiv r4_5
    fst  .Ixx
    fst  .Iyy
    fstp .Izz
    mov .dBodyType,BODY_TYPE_SPHERE
MethodEnd


And while we're at it, here's the InitMesh method, which merely calls the ancestor D3D_Mesh.Init method, we need this if we wish to apply accurate physics to complex meshes.

Method RigidBody.InitMesh,uses esi,pOwner,pD3D,pTexManager
    SetObject esi
    ACall esi.Init, pOwner,pD3D,pTexManager
MethodEnd


Voila, inertia tensor for any kind of solid sphere, based on mass and radius.
With the exception of the diagonalise method, we've pretty much got our body initializing code in place.
It's worth mentioning that I'll be extending the D3D material structure to add physical attributes to Materials which are then available for application to groups of faces of a given mesh.
We'll be able to describe the physical texture of a surface in more detail than mere optics!
Imagine if our pool table was covered in mud , or someone spilled a sticky drink on it :)

Next we'll take a look at the diagonalise method, then we might take another good stare at our simulator class and start filling some gaps.
See, anyone can do this stuff :)
Posted on 2008-01-24 04:08:10 by Homer
The diagonalise() method is used to find the transform to convert body from BSpace into DSpace. This makes the moment of inertia matrix diagonal, and makes the centre of mass coincide with the Body origin.
The transform from BSpace' to DSpace is :
  Rd = diagRotPos * Ro + diagLinPos
;-------------------------------------------------------------------------------------


Method RigidBody.diagonalise,uses esi
LOCAL identity:Mat33
LOCAL I:Mat33 ; moment of inertia (old coords)
LOCAL cOfM:Vec3
Mat33_identity identity
 
    ;Vector3 cOfM(-Ix / mass, -Iy / mass, -Iz / mass);  // centre of mass in old coords
    SetObject esi
    fld .Ix
    fchs
    fmul .oneOverMass
    fstp cOfM.x
    ;
    fld .Iy
    fchs
    fmul .oneOverMass
    fstp cOfM.y
;
    fld .Iz
    fchs
    fmul .oneOverMass
    fstp cOfM.z
   
    OCall clear
    OCall combine, esi, addr identity, addr cOfM ; shift centre of mass to origin

        ;Build BSpace tensor
        ;Ixx, -Ixy, -Izx,
        ;-Ixy, Iyy, -Iyz,
        ;-Izx, -Iyz, Izz
       
        m2m I.M0,.Ixx
  fld .Ixy
  fchs
  fstp I.M1
  fld .Izx
  fchs
  fstp I.M2
  ;
  fld .Ixy
  fchs
  fstp I.M3
  m2m I.M4, .Iyy
  fld .Iyz
  fchs
  fstp I.M5
  ;
  fld .Izx
  fchs
  fstp I.M6
  fld .Iyz
  fchs
  fstp I.M7
  m2m I.M8, .Izz
   
    ; find diagonalising transform with eigen vectors
    ; this is the transform that would get us from DSpace back into BSpace.
    lea edx,.diagIxx ;ptr to target vecx, ie, Ixx Iyy Izz are all affected
    invoke Mat33_eigenvectors,addr .diagRotPos,edx,addr I 
    ; We actually want the inverse (from BSpace to DSpace).
    ; Since the matrix is a rotation matrix, we can use transpose ()
    Mat33_transpose .diagRotPos

    ;Calculate the Body's position in DSpace
    ;diagLinPos = diagRotPos * cOfM
    Vec3_mult_Mat33 .diagRotPos, cOfM

; now precalculate some useful numbers for speed ups
fld1
fdiv .diagIxx
fstp .oneOverDiagIxx
fld1
fdiv .diagIyy
fstp .oneOverDiagIyy
fld1
fdiv .diagIzz
fstp .oneOverDiagIzz
fld1
fdiv .mass
fstp .oneOverMass
fld .mass
fchs
fmul g
fstp .minusMG
fld  .diagIyy
fsub .diagIzz
fdiv .diagIxx
fstp .diagIyyMinusIzzOverIxx
fld  .diagIzz
fsub .diagIxx
fdiv .diagIyy
fstp .diagIzzMinusIxxOverIyy
fld  .diagIxx
fsub .diagIyy
fdiv .diagIzz
fstp .diagIxxMinusIyyOverIzz
fld  .diagIzz
fsub .diagIyy
fstp .diagIzzMinusIyy
fld  .diagIxx
fsub .diagIzz
fstp .diagIxxMinusIzz
fld  .diagIyy
fsub .diagIxx
fstp .diagIyyMinusIxx
MethodEnd


Mat33_eigenvectors is the heart of this method.
It diagonalizes the matrix using "Jacobian rotation", where we "rotate away" the nonzero components of the tensor, and simultaneously builds a matrix which would "undo" all those rotations.
The result is that we get a diagonal matrix, and the inverse matrix that would convert the diagonal matrix into the original one.

I won't show that code here, I will simply promise that it's in the sourcecode archive which will (soon) be attached to this document.

Now assuming that we can supply the inertia tensor of an arbitrary body, we can now produce a DSpace tensor, and matrices to transform between DSpace and BSpace, for ANY arbitrary Body.
The beauty about working in DSpace is that since our tensor only contains three values, we can treat the tensor as a Vector, which is a huge speedup when we want to use it in our math :)



Posted on 2008-01-24 06:56:43 by Homer
Let's leave the RigidBody class alone for a while, and get back to the simulator class.
I've added a little more meat to the bare bones presented so far.
The main thing that should catch your eye is that the simulator class supports Springs, via Hooke's Law.
A Spring is a Constraint that we can apply to a pair of Bodies.
As its name suggests, a Spring can be stiff and rigid, or soft and flexible, but either way, its job is to maintain the distance between two anchor points on two Bodies, irrespective of their orientations.
We can also anchor one end of a Spring to a point in WorldSpace, effectively 'tethering' the Body.
Springs are useful for ALL KINDS of things, and are an excellent introduction into the application of physical constraints (yes, theres many other kinds I won't talk about) in our simulation.

I've described Springs as a structure which keeps track of the two anchor points and who owns them.
A Spring knows which Body or Bodies it affects, not the other way around.
That's why I've implemented Springs here in the simulator class rather than somewhere else.
In fact, I've described two kinds : WorldSpring and BodySpring, using separate structs.

Of course, we don't NEED springs, but having them is very nice.
Hey, we don't need Gravity either, but you can't make a necklace out of Gravity, or do pendulum stuff, or make a rope bridge, etc etc.

The methods that mention "random bodies" are only there for demonstration purposes... they'll help us get an interesting demo together more quickly.


Object Simulator,sid,Collection
RedefineMethod Init, Pointer,Pointer,Pointer, real4,real4,real4, dword ;pOwner,pD3D,pTexManager, fWorldX, fWorldY, fWorldZ, dNumRandomBodies
RedefineMethod Done
StaticMethod Simulate, real4 ;fDeltaTime
StaticMethod Create_Spring_WorldBody,LPVEC3,dword,dword ;pvAnchor, iBody, iHullVertex
StaticMethod Create_Spring_BodyBody,dword,dword,dword,dword ;iBodyA, iHullVertexA, iBodyB, iHullVertexB
StaticMethod GetTransform,LPD3DXMATRIX,dword
   
Private
StaticMethod InitializeRandomBodies,dword
StaticMethod ComputeForces
StaticMethod Integrate,real4 ;fDeltaTime
StaticMethod CheckForCollisions
StaticMethod ResolveCollision,LPBODY,LPVEC3,LPVEC3 ;pTouchingBody,pPointOnBody,pCollisionNormal
StaticMethod CalculateVertices,dword ;dStateIndex
StaticMethod Update
PrivateEnd

DefineVariable GravityActive, BOOL, TRUE
DefineVariable DampingActive, BOOL, FALSE
DefineVariable BodySpringsActive, BOOL, FALSE
DefineVariable WorldSpringsActive, BOOL, FALSE
DefineVariable Gravity, real4, -0.86f
DefineVariable CollisionNormal, Vec3, {<>}
DefineVariable NumberOfWalls, dword,6
DefineVariable NumberOfBodySprings, dword,NULL
DefineVariable NumberOfWorldSprings,dword,NULL
Embed BodySprings,  DataCollection
Embed WorldSprings, DataCollection
ObjectEnd


The ancestor collection is used to keep a list of all the Bodies in the simulation.
The embedded datacollections keep arrays of springs.
And I think we should take a closer look at springs next, so let's check out some structures:


;The 3x3 Matrix structure, if you wish to address as 1D array
Mat33 struct
M0 real4 ?
M1 real4 ?
M2 real4 ?
M3 real4 ?
M4 real4 ?
M5 real4 ?
M6 real4 ?
M7 real4 ?
M8 real4 ?
Mat33 ends

;The 3x3 Matrix structure, if you wish to address as 2D array
mat33 struct
M00 real4 ?
M01 real4 ?
M02 real4 ?
M10 real4 ?
M11 real4 ?
M12 real4 ?
M20 real4 ?
M21 real4 ?
M22 real4 ?
mat33 ends

;World Springs tether a Body to a fixed point in WorldSpace.
world_spring struct
pBody dd ? ;Body this Spring is attached to
    BodyAnchor  Vec3 <> ;Where in BodySpace this Spring is attached
    HookesConstant real4 ?
    WorldAnchor Vec3 <> ;Where in WorldSpace this Spring is anchored
    Dampening real4 ?
world_spring ends

;Body Springs tether two Bodies to each other.
;Again, attachment points are existing boundingvertices only.
body_spring struct
pBodyA dd ? ;Body A
pBodyB dd ? ;Body B
    BodyAnchorA Vec3 <> ;Where in BSpace this Spring was attached to on Body A
    HookesConstant real4 ?
    BodyAnchorB Vec3 <> ;Where in BSpace this Spring was attached to on Body B 
    Dampening real4 ?
body_spring ends



Note that our Springs support individual settings for "flexibility" (Hookes Const.) and "stiffness" (Dampening). By adding these, we get a lot more control if/when/where we want it, and it also causes our Vector fields to be aligned on dword boundaries, which in turn yields a nice speedup by preventing a performance penalty on some architectures.





Posted on 2008-01-24 07:47:24 by Homer
I want to talk more about collision detection now.
Our first demo will not use collision detection at all, but I'd still like to present it now.


;Check all Bodies in the Simulator for collisions, and Resolve all Collisions.
Method Simulator.CheckForCollisions,uses esi ecx
LOCAL CollisionState,Counter
LOCAL vCollisionPoint:Vec3 ;in worldspace, relative to body origin
LOCAL vCollisionNormal:Vec3 ;for a wall, its the surface normal

SetObject esi
    mov CollisionState , Clear     ; be optimistic!
       
;Checking each Body in the Simulator which is NOT "at rest"
;to find if it has collided with ANYTHING (at rest or not).
xor ecx,ecx
.while ecx < .dCount && CollisionState!=Penetrating
push ecx
shl ecx,2
add ecx,.pItems
OCall CheckBodyForCollisions,dword ptr,addr vCollisionPoint ,addr CollisionNormal
                .if CollisionState == Clear
                      mov CollisionState ,eax
                .elseif  eax==Touching
                        ;The Bodies are kissing - We can calculate the response force now
mov Counter,0
.repeat         
OCall ResolveCollision,dword ptr,pvCollisionPointOut,pvCollisionNormalOut
          mov CollisionState,$OCall (CheckBodyForCollisions,dword ptr,addr vCollisionPoint,addr vCollisionNormal)
            .break .if eax!=Penetrating|| Counter==100
                inc Counter
          .until 0
.if Counter==100
DbgWarning "Error - failed to resolve Collisions in 100 attempts"
int 3
.endif
                .else
                    ;The Bodies are Penetrating - we'll let Simulator search for their kissing-time and tag them.
.endif
        pop ecx
        inc ecx
.endw

return CollisionState
;MethodEnd


Each Body will be checked for collisions against all other Bodies.
The code used to actually perform Body-Body tests is optimized (mainly, according to the types of bodies involved).
The ResolveCollision() method is used to accumulate collision forces in our Bodies (multiple contacts), and when testing is completed, this method will tell its caller whether ANY collisions occurred, and if so, the caller can then apply the accumulated forces to the bodies.
This implies that each Body will have to store its own "kissing time", something I'll have to address !!
Resolving a collision implies that we know where and when the collision occurred, and this method will be among the first to know that information.
Resolving a collision can result in the bodies being moved clear of one another, or just touching.
This is important both for finding the kissing point, and for "rolling contact" :)



Posted on 2008-01-24 08:20:12 by Homer
Now we'll turn our attention back to the RigidBody class again.
Let's take a look at those mysterious RK methods, starting with RKInit()
Its purpose is to find the "current forces" acting on the Body (linForce and rotForce), which we can obtain from the current Momentum (linear and angular).
In fact we don't even need to know the Momentum, because it is a product of Velocity (again, linear and angular).
F = ma , and a = dv/dt



;==================================================================================
Method RigidBody.RKInit,uses esi
SetObject esi

;save position before step is made
m2m .linPos0.x,.linPos.x
m2m .linPos0.y,.linPos.y
m2m .linPos0.z,.linPos.z
m2m .qRotPos0.x,.qRotPos.x
m2m .qRotPos0.y,.qRotPos.y
m2m .qRotPos0.z,.qRotPos.z
m2m .qRotPos0.w,.qRotPos.w
invoke RtlMoveMemory,addr .rotPos0,addr .rotPos,sizeof Mat33
m2m .linVel0.x,.linVel.x
m2m .linVel0.y,.linVel.y
m2m .linVel0.z,.linVel.z
m2m .rotVel0.x,.rotVel.x
m2m .rotVel0.y,.rotVel.y
m2m .rotVel0.z,.rotVel.z

; built in damping and gravity
fld .linKD
fmul .linVel.x
fstp .linForce.x

fld .linKD
fmul .linVel.y
fadd .minusMG
fstp .linForce.y

fld .linKD
fmul .linVel.z
fstp .linForce.z

;
fld .rotKDx
fmul .rotVel.x
fstp .rotForce.x
fld .rotKDy
fmul .rotVel.y
fstp .rotForce.y
fld .rotKDz
fmul .rotVel.z
fstp .rotForce.z
MethodEnd


If we know the current Forces acting on the Body, we can apply them to find the new (linear and angular) velocity, position, orientation, whatever we want.
RK integration requires four steps, Euler integration just one.
Let's see the code for performing the first 3 RK steps.
This method is used to advance the system.
Given an input state and input forces, it produces an output state and output forces.
Each step feeds its results into the next step.
Since the code for the first three steps is so similar, I have written it as one method with casecode.
The fourth step is somewhat different.




Method RigidBody.RKStep123,uses esi, iStep
local l2, ftemp

SetObject esi

fld  .linForce.x
fmul RKh2
fmul .oneOverMass
fld  .rotVel.y
fmul .rotVel.z
fmul .diagIyyMinusIzzOverIxx
fld  .rotForce.x
fmul .oneOverDiagIxx
fadd
fmul RKh2

fld  .linForce.y
fmul RKh2
fmul .oneOverMass
fld  .rotVel.z
fmul .rotVel.x
fmul .diagIzzMinusIxxOverIyy
fld  .rotForce.y
fmul .oneOverDiagIyy
fadd
fmul RKh2

fld  .linForce.z
fmul RKh2
fmul .oneOverMass
fld  .rotVel.x
fmul .rotVel.y
fmul .diagIxxMinusIyyOverIzz
fld  .rotForce.z
fmul .oneOverDiagIzz
fadd
fmul RKh2

.switch iStep
.case 1
    ;rotAcc1.x = (rotVel.y * rotVel.z * diagIyyMinusIzzOverIxx + rotForce.x * oneOverDiagIxx) * RKh2;
    ;rotAcc1.y = (rotVel.z * rotVel.x * diagIzzMinusIxxOverIyy + rotForce.y * oneOverDiagIyy) * RKh2;
    ;rotAcc1.z = (rotVel.x * rotVel.y * diagIxxMinusIyyOverIzz + rotForce.z * oneOverDiagIzz) * RKh2;
    ;linAcc1 = linForce * oneOverMass * RKh2
    fstp .rotAcc1.z
    fstp .rotAcc1.y
    fstp .rotAcc1.x
    fstp .linAcc1.z
    fstp .linAcc1.y
    fstp .linAcc1.x
    fUnload

    ;linVel1 = linVel
    ;rotVel1 = rotVel
    m2m .linVel1.x,.linVel.x
    m2m .linVel1.y,.linVel.y
    m2m .linVel1.z,.linVel.z
    m2m .rotVel1.x,.rotVel.x
    m2m .rotVel1.y,.rotVel.y
    m2m .rotVel1.z,.rotVel.z

    ;linPos += RKh2 * linVel1
    fld .linVel1.x
    fmul RKh2
    fld .linVel1.y
    fmul RKh2
    fld .linVel1.z
    fmul RKh2
    fadd .linPos.z
    fstp .linPos.z
    fadd .linPos.y
    fstp .linPos.y    
    fadd .linPos.x
    fstp .linPos.x
       
    ;qRotPos.w -= RKh4 * (qRotPos0.x * rotVel1.x + qRotPos0.y * rotVel1.y + qRotPos0.z * rotVel1.z);
    ;qRotPos.x += RKh4 * (qRotPos0.w * rotVel1.x - qRotPos0.z * rotVel1.y + qRotPos0.y * rotVel1.z);
    ;qRotPos.y += RKh4 * (qRotPos0.z * rotVel1.x + qRotPos0.w * rotVel1.y - qRotPos0.x * rotVel1.z);
    ;qRotPos.z += RKh4 * (qRotPos0.x * rotVel1.y - qRotPos0.y * rotVel1.x + qRotPos0.w * rotVel1.z);
    fld .qRotPos0.x
    fmul .rotVel1.x
    fld .qRotPos0.y
    fmul .rotVel1.y
    fld .qRotPos0.z
    fmul .rotVel1.z
    fadd
    fadd
    fmul RKh4
    fstp .qRotPos.w
    ;
    fld .qRotPos0.w
    fmul .rotVel1.x
    fld .qRotPos0.z
    fmul .rotVel1.y
    fsub
    fld .qRotPos0.y
    fmul .rotVel1.z
    fadd
    fmul RKh4
    fstp .qRotPos.x
    ;
    fld .qRotPos0.x
    fmul .rotVel1.y
    fld .qRotPos0.y
    fmul .rotVel1.x
    fsub
    fld .qRotPos0.w
    fmul .rotVel1.z
    fadd
    fmul RKh4
    fstp .qRotPos.z
;
    fld .qRotPos0.z
    fmul .rotVel1.x
    fld .qRotPos0.w
    fmul .rotVel1.y
    fadd
    fld .qRotPos0.x
    fmul .rotVel1.z
    fsub
    fmul RKh4
    fstp .qRotPos.y   
   
    ;linVel += linAcc1
    ;rotVel += rotAcc1
    fld .linAcc1.x
    fadd .linVel.x
    fld .linAcc1.y
    fadd .linVel.y
    fld .linAcc1.z
    fadd .linVel.z
    fld .rotAcc1.x
    fadd .rotVel.x
    fld .rotAcc1.y
    fadd .rotVel.y    
    fld .rotAcc1.z
    fadd .rotVel.z

.case 2
    ;rotAcc2.x = (rotVel.y * rotVel.z * diagIyyMinusIzzOverIxx + rotForce.x * oneOverDiagIxx) * RKh2;
    ;rotAcc2.y = (rotVel.z * rotVel.x * diagIzzMinusIxxOverIyy + rotForce.y * oneOverDiagIyy) * RKh2;
    ;rotAcc2.z = (rotVel.x * rotVel.y * diagIxxMinusIyyOverIzz + rotForce.z * oneOverDiagIzz) * RKh2;
    ;linAcc2 = linForce * oneOverMass * RKh2
    fstp .rotAcc2.z
    fstp .rotAcc2.y
    fstp .rotAcc2.x
    fstp .linAcc2.z
    fstp .linAcc2.y
    fstp .linAcc2.x
    fUnload

    ;linVel2 = linVel;
    ;rotVel2 = rotVel;
    m2m .linVel2.x,.linVel.x
    m2m .linVel2.y,.linVel.y
    m2m .linVel2.z,.linVel.z
    m2m .rotVel2.x,.rotVel.x
    m2m .rotVel2.y,.rotVel.y
    m2m .rotVel2.z,.rotVel.z    
   
    ;linPos = linPos0 + RKh2 * linVel2
    fld RKh2
    fld .linVel2.x
    fmul RKh2
    fld .linVel2.y
    fmul RKh2
    fld .linVel2.z
    fmul RKh2
    fadd .linPos0.z
    fstp .linPos.z
    fadd .linPos0.y
    fstp .linPos.y    
    fadd .linPos0.x
    fstp .linPos.x

    ;qRotPos.w = qRotPos0.w - RKh4 * (qRotPos0.x * rotVel2.x + qRotPos0.y * rotVel2.y + qRotPos0.z * rotVel2.z);
    ;qRotPos.x = qRotPos0.x + RKh4 * (qRotPos0.w * rotVel2.x - qRotPos0.z * rotVel2.y + qRotPos0.y * rotVel2.z);
    ;qRotPos.y = qRotPos0.y + RKh4 * (qRotPos0.z * rotVel2.x + qRotPos0.w * rotVel2.y - qRotPos0.x * rotVel2.z);
    ;qRotPos.z = qRotPos0.z + RKh4 * (qRotPos0.x * rotVel2.y - qRotPos0.y * rotVel2.x + qRotPos0.w * rotVel2.z);
fld  .qRotPos0.w
fld  .qRotPos0.x
fmul .rotVel2.x
fld  .qRotPos0.y
fmul .rotVel2.y
fld  .qRotPos0.z
fmul .rotVel2.z
fadd
fadd
fmul RKh4
fsub
fstp .qRotPos.w
;
fld  .qRotPos0.w
fmul .rotVel2.x
fld  .qRotPos0.z
fmul .rotVel2.y
fsub
fld  .qRotPos0.y
fmul .rotVel2.z
fadd
fmul RKh4
fadd .qRotPos0.x
fstp .qRotPos.x
;
fld  .qRotPos0.z
fmul .rotVel2.x
fld  .qRotPos0.w
fmul .rotVel2.y
fadd
fld  .qRotPos0.z
fmul .rotVel2.z
fsub
fmul RKh4
fadd .qRotPos0.y
fstp .qRotPos.y
;
fld  .qRotPos0.x
fmul .rotVel2.y
fld  .qRotPos0.y
fmul .rotVel2.x
fsub
fld  .qRotPos0.w
fmul .rotVel2.z
fadd
fmul RKh4
fadd .qRotPos0.z
fstp .qRotPos.z

    ;linVel = linVel1 + linAcc2
    ;rotVel = rotVel1 + rotAcc2
    fld .linAcc2.x
    fadd .linVel1.x
    fld .linAcc2.y
    fadd .linVel1.y
    fld .linAcc2.z
    fadd .linVel1.z
    fld .rotAcc2.x
    fadd .rotVel1.x
    fld .rotAcc2.y
    fadd .rotVel1.y    
    fld .rotAcc2.z
    fadd .rotVel1.z

.case 3
    ;rotAcc3.x = (rotVel.y * rotVel.z * diagIyyMinusIzzOverIxx + rotForce.x * oneOverDiagIxx) * RKh;
    ;rotAcc3.y = (rotVel.z * rotVel.x * diagIzzMinusIxxOverIyy + rotForce.y * oneOverDiagIyy) * RKh;
    ;rotAcc3.z = (rotVel.x * rotVel.y * diagIxxMinusIyyOverIzz + rotForce.z * oneOverDiagIzz) * RKh;
    ;linAcc3 = linForce * oneOverMass * RKh
    fstp .rotAcc3.z
    fstp .rotAcc3.y
    fstp .rotAcc3.x
    fstp .linAcc3.z
    fstp .linAcc3.y
    fstp .linAcc3.x
    fUnload
   
    ;linVel3 = linVel
    ;rotVel3 = rotVel
    m2m .linVel3.x,.linVel.x
    m2m .linVel3.y,.linVel.y
    m2m .linVel3.z,.linVel.z
    m2m .rotVel3.x,.rotVel.x
    m2m .rotVel3.y,.rotVel.y
    m2m .rotVel3.z,.rotVel.z  

    ;linPos = linPos0 + RKh * linVel3
    fld RKh
    fld .linVel3.x
    fmul RKh
    fld .linVel3.y
    fmul RKh
    fld .linVel3.z
    fmul RKh
    fadd .linPos0.z
    fstp .linPos.z
    fadd .linPos0.y
    fstp .linPos.y    
    fadd .linPos0.x
    fstp .linPos.x
   
    ;qRotPos.w = qRotPos0.w - RKh2 * (qRotPos0.x * rotVel3.x + qRotPos0.y * rotVel3.y + qRotPos0.z * rotVel3.z);
    ;qRotPos.x = qRotPos0.x + RKh2 * (qRotPos0.w * rotVel3.x - qRotPos0.z * rotVel3.y + qRotPos0.y * rotVel3.z);
    ;qRotPos.y = qRotPos0.y + RKh2 * (qRotPos0.z * rotVel3.x + qRotPos0.w * rotVel3.y - qRotPos0.x * rotVel3.z);
    ;qRotPos.z = qRotPos0.z + RKh2 * (qRotPos0.x * rotVel3.y - qRotPos0.y * rotVel3.x + qRotPos0.w * rotVel3.z);
    fld  .qRotPos0.w
    fld  .qRotPos0.x
    fmul .rotVel3.x
    fld  .qRotPos0.y
    fmul .rotVel3.y
    fld  .qRotPos0.z
    fmul .rotVel3.z
    fadd
    fadd
    fmul RKh2
    fsub
    fstp .qRotPos.w
    ;
    fld  .qRotPos0.w
    fmul .rotVel3.x
    fld  .qRotPos0.z
    fmul .rotVel3.y
fsub
    fld  .qRotPos0.y
    fmul .rotVel3.z
    fadd
    fmul RKh2
    fadd .qRotPos0.x
    fstp .qRotPos.x
;
    fld  .qRotPos0.z
    fmul .rotVel3.x
    fld  .qRotPos0.w
    fmul .rotVel3.y
fadd
    fld  .qRotPos0.x
    fmul .rotVel3.z
    fsub
    fmul RKh2
    fadd .qRotPos0.y
    fstp .qRotPos.y
    ;
    fld  .qRotPos0.x
    fmul .rotVel3.y
    fld  .qRotPos0.y
    fmul .rotVel3.x
fsub
    fld  .qRotPos0.w
    fmul .rotVel3.z
    fadd
    fmul RKh2
    fadd .qRotPos0.z
    fstp .qRotPos.z
   
    ;linVel = linVel1 + linAcc3
    ;rotVel = rotVel1 + rotAcc3
    fld .linAcc3.x
    fadd .linVel1.x
    fld .linAcc3.y
    fadd .linVel1.y
    fld .linAcc3.z
    fadd .linVel1.z
    fld .rotAcc3.x
    fadd .rotVel1.x
    fld .rotAcc3.y
    fadd .rotVel1.y    
    fld .rotAcc3.z
    fadd .rotVel1.z
   
.endsw
fstp .rotVel.z
fstp .rotVel.y
fstp .rotVel.x
fstp .linVel.z
fstp .linVel.y
fstp .linVel.x

Quaternion_normalize .qRotPos
    ;rotPos.fromQuatL2(qRotPos, l2);                // make new orthogonal matrix from quaternion.
invoke Mat33_fromQuatL2,addr .rotPos,addr .qRotPos, l2

;linForce.x = linKD * linVel.x;                  // built in damping and gravity
    ;linForce.y = linKD * linVel.y+ minusMG;
    ;linForce.z = linKD * linVel.z
;rotForce.x = rotKDx * rotVel.x;
;rotForce.y = rotKDy * rotVel.y;
;rotForce.z = rotKDz * rotVel.z;

fld .linKD
fmul .linVel.x
fstp .linForce.x
fld .linKD
fmul .linVel.y
fadd .minusMG
fstp .linForce.y
fld .linKD
fmul .linVel.z
fstp .linForce.z

fld .rotKDx
fmul .rotVel.x
fstp .rotForce.x
fld .rotKDy
fmul .rotVel.y
fstp .rotForce.y
fld .rotKDz
fmul .rotVel.z
fstp .rotForce.z
MethodEnd


Posted on 2008-01-25 20:37:42 by Homer
Hi Homer
I got some time to read the whole post, but now I have. It is an excellent work  :thumbsup:. While reading it, some questions come to my mind.


  • 1. When we create our world entities, we define their mass and geometrical properties like weight, inertia tensor, etc. All these are done outside the application in a sort of design environment, so we donít have to compute it so we can concentrate on collisions and animations. Am I right with this assumption? If yes, do you know of such an environment?

  • 2. Iím a little unsure if it is possible synchronize the physics simulation with the rendering while we have no contact of our entities. It would be my first approach. Iím sure if this is what you suggest at the top of your second post, before we began to split the time slice to find the contact (kissing) moment.

  • 3. You say you cumulate forces and donít apply them immediately, When do you apply them? At the end of a physic simulation time slice (= rendering?)

  • 4. In the simulator object you tell us about springs. Are they always connected to the entities or is there a way to say them to be ďactiveĒ depending on the distance between the entities?

  • 5. Iím interested to know how you resolve the collision of entities. Are you using a sort of BSP/Octree algo for this?



Regards,

Biterider

Posted on 2008-01-26 01:38:45 by Biterider

    * 1. When we create our world entities, we define their mass and geometrical properties like weight, inertia tensor, etc. All these are done outside the application in a sort of design environment, so we donít have to compute it so we can concentrate on collisions and animations. Am I right with this assumption? If yes, do you know of such an environment?

You can calculate all of these values at initialization time for any arbitrary body.
No environment I know of will hand you these values.
The only thing you can't calculate is Density, and you can look up the density of any material on the web.


    * 2. Iím a little unsure if it is possible synchronize the physics simulation with the rendering while we have no contact of our entities. It would be my first approach. Iím sure if this is what you suggest at the top of your second post, before we began to split the time slice to find the contact (kissing) moment.

It is not desirable to synchronize rendering and physics!
Usually, we would like to perform several physics passes for each rendered frame - but if this takes too long, we find ourselves rendering more than one frame per physics update, which is USELESS - because "nothing has moved". If we have a choice about how we spend our spare time, it should not be wasted by rendering the SAME FRAME of video if nothing has moved!


    * 3. You say you cumulate forces and donít apply them immediately, When do you apply them? At the end of a physic simulation time slice (= rendering?)

We should apply the accumulated forces in RK Step#1, so that they are considered throughout the physics timestep, and alter the output state.
In the code I have shown that we calculate "current forces" in the RKInit method, but I have not shown that force being added to the existing force, which is only because the code I am presenting was designed for single contacts, we'll correct this as we go, well spotted.


    * 4. In the simulator object you tell us about springs. Are they always connected to the entities or is there a way to say them to be ďactiveĒ depending on the distance between the entities?

Springs, like anything, can be enabled or disabled. Our springs so far allow you to define the distance that you wish to maintain, the maximum amount of stretch, and the maximum amount of compression.
You don't have to use them at all, but later, we will use them in simple demos for pendulums and ropes, and later still, we can use them in articulated bodies (ragdolls).
They are, without doubt, the most useful and powerful "constraint" we will ever need.


    * 5. Iím interested to know how you resolve the collision of entities. Are you using a sort of BSP/Octree algo for this?

Collision resolution involves the calculation of the moment of impact, the "collision normal", and the force generated by the collision (based on velocity and mass of the entities involved).
We calculate the resulting force for each body, then we shove it into each body's force accumulator, where it will linger until we update that body's state.

Posted on 2008-01-26 03:08:06 by Homer
Here's the Rk Step #4 method


Method RigidBody.RKStep4,uses esi
LOCAL rotVel4:Vec3
LOCAL l2, ftemp
SetObject esi

    ;linPos = linPos0 + (linVel1 + 2.0 * (linVel2 + linVel3) + linVel) * RKh6 
    fld  .linVel2.x
    fadd .linVel3.x
    fadd st(0),st(0)
    fadd .linVel.x
    fadd .linVel1.x
    fmul RKh6
    fadd .linPos0.x
    fstp .linPos.x
    ;
    fld  .linVel2.y
    fadd .linVel3.y
    fadd st(0),st(0)
    fadd .linVel.y
    fadd .linVel1.y
    fmul RKh6
    fadd .linPos0.y
    fstp .linPos.y
    ;
    fld  .linVel2.z
    fadd .linVel3.z
    fadd st(0),st(0)
    fadd .linVel.z
    fadd .linVel1.z
    fmul RKh6
    fadd .linPos0.z
    fstp .linPos.z   
   
    ;rotVel4 = (rotVel1 + rotVel) * 0.5 + rotVel2 + rotVel3
    fld  .rotVel1.x
    fadd .rotVel.x
    fmul r4_Half
    fadd .rotVel2.x
    fadd .rotVel3.x
    fstp rotVel4.x
    ;
    fld  .rotVel1.y
    fadd .rotVel.y
    fmul r4_Half
    fadd .rotVel2.y
    fadd .rotVel3.y
    fstp rotVel4.y
    ;
    fld  .rotVel1.z
    fadd .rotVel.z
    fmul r4_Half
    fadd .rotVel2.z
    fadd .rotVel3.z
    fstp rotVel4.z
   
    ;qRotPos.w = qRotPos0.w - RKh6 * (qRotPos0.x * rotVel4.x + qRotPos0.y * rotVel4.y + qRotPos0.z * rotVel4.z);
    ;qRotPos.x = qRotPos0.x + RKh6 * (qRotPos0.w * rotVel4.x - qRotPos0.z * rotVel4.y + qRotPos0.y * rotVel4.z);
    ;qRotPos.y = qRotPos0.y + RKh6 * (qRotPos0.z * rotVel4.x + qRotPos0.w * rotVel4.y - qRotPos0.x * rotVel4.z);
    ;qRotPos.z = qRotPos0.z + RKh6 * (qRotPos0.x * rotVel4.y - qRotPos0.y * rotVel4.x + qRotPos0.w * rotVel4.z);
fld  .qRotPos0.w
fld  .qRotPos.x
fmul rotVel4.x
fld  .qRotPos.y
fmul rotVel4.y
fld  .qRotPos.z
fmul rotVel4.z
fadd
fadd
fmul RKh6
fsub
fstp .qRotPos.w
;
fld  .qRotPos0.w
fmul rotVel4.x
fld  .qRotPos0.z
fmul rotVel4.y
fsub
fld  .qRotPos0.y
fadd rotVel4.z
fadd
fmul RKh6
fadd .qRotPos0.x
fstp .qRotPos.x
;
fld  .qRotPos0.z
fmul rotVel4.x
fld  .qRotPos0.w
fmul rotVel4.y
fadd
fld  .qRotPos0.x
fadd rotVel4.z
fsub
fmul RKh6
fadd .qRotPos0.y
fstp .qRotPos.y
;
fld  .qRotPos0.x
fmul rotVel4.y
fld  .qRotPos0.y
fmul rotVel4.x
fsub
fld  .qRotPos0.w
fadd rotVel4.z
fadd
fmul RKh6
fadd .qRotPos0.z
fstp .qRotPos.z

Quaternion_normalize .qRotPos
    ;rotPos.fromQuatL2(qRotPos, l2);                // make new orthogonal matrix from quaternion.
invoke Mat33_fromQuatL2,addr .rotPos,addr .qRotPos, l2

  ;linVel = linVel1 + (linAcc1 + 2.0 * linAcc2 + linAcc3 + linForce * oneOverMass * RKh2) * (1.0 / 3.0)
  fld .linAcc2.x
  fadd st(0),st(0)
  fadd .linAcc1.x
  fadd .linAcc3.x
  fld .linForce.x
  fmul .oneOverMass
  fmul RKh2
  fadd
  fdiv r4_3
  fadd .linVel1.x
  fstp .linVel.x
  ;
  fld .linAcc2.y
  fadd st(0),st(0)
  fadd .linAcc1.y
  fadd .linAcc3.y
  fld .linForce.y
  fmul .oneOverMass
  fmul RKh2
  fadd
  fdiv r4_3
  fadd .linVel1.y
  fstp .linVel.y
  ;
  fld .linAcc2.z
  fadd st(0),st(0)
  fadd .linAcc1.z
  fadd .linAcc3.z
  fld .linForce.z
  fmul .oneOverMass
  fmul RKh2
  fadd
  fdiv r4_3
  fadd .linVel1.z
  fstp .linVel.z

  ;rotVel4.x = rotVel1.x + (rotAcc1.x + 2.0 * rotAcc2.x + rotAcc3.x + (rotVel.y * rotVel.z * diagIyyMinusIzzOverIxx + rotForce.x * oneOverDiagIxx) * RKh2) * (1.0 / 3.0);
  fld .rotAcc2.x
  fadd st(0),st(0)
  fadd .rotAcc1.x
  fadd .rotAcc3.x
  fld  .rotVel.y
  fmul .rotVel.z
  fmul .diagIyyMinusIzzOverIxx
  fld .rotForce.x
  fmul .oneOverDiagIxx
  fadd
  fadd
  fmul RKh2
  fdiv r4_3
  fadd .rotVel1.x
  fstp rotVel4.x
 
  ;rotVel4.y = rotVel1.y + (rotAcc1.y + 2.0 * rotAcc2.y + rotAcc3.y + (rotVel.z * rotVel.x * diagIzzMinusIxxOverIyy + rotForce.y * oneOverDiagIyy) * RKh2) * (1.0 / 3.0);
  fld .rotAcc2.y
  fadd st(0),st(0)
  fadd .rotAcc1.y
  fadd .rotAcc3.y
  fld  .rotVel.z
  fmul .rotVel.x
  fmul .diagIzzMinusIxxOverIyy
  fld .rotForce.y
  fmul .oneOverDiagIyy
  fadd
  fadd
  fmul RKh2
  fdiv r4_3
  fadd .rotVel1.y
  fstp rotVel4.y

  ;rotVel4.z = rotVel1.z + (rotAcc1.z + 2.0 * rotAcc2.z + rotAcc3.z + (rotVel.x * rotVel.y * diagIxxMinusIyyOverIzz + rotForce.z * oneOverDiagIzz) * RKh2) * (1.0 / 3.0);
  fld .rotAcc2.z
  fadd st(0),st(0)
  fadd .rotAcc1.z
  fadd .rotAcc3.z
  fld  .rotVel.x
  fmul .rotVel.y
  fmul .diagIxxMinusIyyOverIzz
  fld .rotForce.z
  fmul .oneOverDiagIzz
  fadd
  fadd
  fmul RKh2
  fdiv r4_3
  fadd .rotVel1.z
  fstp rotVel4.z


    ;rotVel = rotVel4
    m2m .rotVel.x, rotVel4.x
    m2m .rotVel.y, rotVel4.y
    m2m .rotVel.z, rotVel4.z
   
MethodEnd


When we have called 4 RK steps, we have a new position (of COM), new velocities (linear and angular), and a new orientation (quaternion).
We can now render our body :)

Posted on 2008-01-26 03:23:06 by Homer
Just for comparison's sake, and for those of you who are uncomfortable with the RK four-step dance, here is the Euler equivalent of the four RK steps:


Method RigidBody.eulerStep,uses esi
LOCAL l2, ftemp

SetObject esi

    ;linPos += RKh * linVel
    ;linVel += linForce * oneOverMass * RKh   
    fld .linVel.x
    fld .linVel.y
    fld .linVel.z
    fld RKh
    fmul st(1),st(0)
    fmul st(2),st(0)
    fmul st(3),st(0)
    fUnload
    fadd .linPos.z
    fstp .linPos.z
    fadd .linPos.y
    fstp .linPos.y
    fadd .linPos.x
    fstp .linPos.x
   
    fld .linForce.x
    fld .linForce.y
    fld .linForce.z
    fld RKh
  fmul .oneOverMass
    fmul st(1),st(0)
    fmul st(2),st(0)
    fmul st(3),st(0)
    fUnload
    fadd .linVel.z
    fstp .linVel.z
    fadd .linVel.y
    fstp .linVel.y
    fadd .linVel.x
    fstp .linVel.x

    ;qRotPos.w -= RKh2 * (qRotPos0.x * rotVel.x + qRotPos0.y * rotVel.y + qRotPos0.z * rotVel.z);
    fld .qRotPos.w
    fld  .qRotPos0.x
    fmul .rotVel.x
    fld  .qRotPos0.y
    fmul .rotVel.y
    fadd
    fld  .qRotPos0.z
    fmul .rotVel.z
    fadd
    fmul RKh2
    fsub
    fstp .qRotPos.w
   
    ;qRotPos.x += RKh2 * (qRotPos0.w * rotVel.x - qRotPos0.z * rotVel.y + qRotPos0.y * rotVel.z);
    fld  .qRotPos0.w
    fmul .rotVel.x
    fld  .qRotPos0.z
    fmul .rotVel.y
    fsub
    fld  .qRotPos0.y
    fmul .rotVel.z
    fadd
    fmul RKh2
    fadd .qRotPos.x
    fstp .qRotPos.x
   
    ;qRotPos.y += RKh2 * (qRotPos0.z * rotVel.x + qRotPos0.w * rotVel.y - qRotPos0.x * rotVel.z);
    fld  .qRotPos0.z
    fmul .rotVel.x
    fld  .qRotPos0.w
    fmul .rotVel.y
    fadd
    fld  .qRotPos0.x
    fmul .rotVel.z
    fsub
    fmul RKh2
    fadd .qRotPos.y
    fstp .qRotPos.y

    ;qRotPos.z += RKh2 * (qRotPos0.x * rotVel.y - qRotPos0.y * rotVel.x + qRotPos0.w * rotVel.z);
    fld  .qRotPos0.x
    fmul .rotVel.y
    fld  .qRotPos0.y
    fmul .rotVel.x
    fsub
    fld  .qRotPos0.w
    fmul .rotVel.z
    fadd
    fmul RKh2
    fadd .qRotPos.z
    fstp .qRotPos.z

    ;l2 = qRotPos.x * qRotPos.x + qRotPos.y * qRotPos.y + qRotPos.z * qRotPos.z + qRotPos.w * qRotPos.w;
    fld .qRotPos.x
    fmul st(0),st(0)
    fld .qRotPos.y
    fmul st(0),st(0)   
    fld .qRotPos.z
    fmul st(0),st(0)
    fld .qRotPos.w
    fmul st(0),st(0)
fadd
fadd
fadd
fst l2 ;length of quaternion

fabs ;we want it to be 1.0 length (or really close)
fld1 ;so we'll check to see if we need to Normalize quaternion
fsub
fstp ftemp
fMin ftemp, fEpsilon
fstpReg eax
.if eax==ftemp   
  ; normalise to unit length (approximately)
        ;n = (l2 + 1.0) / (2.0 * l2);
        ;qRotPos.x *= n;
        ;qRotPos.y *= n;
        ;qRotPos.z *= n;
        ;qRotPos.w *= n;
        ;l2 *= n * n;
        fld l2
        fld1
        fadd
        fld l2
        fadd st(0),st(0)
        fdiv
        fstp ftemp
       
        fld .qRotPos.x
        fmul ftemp
        fld .qRotPos.y
        fmul ftemp
        fld .qRotPos.z
        fmul ftemp
        fld .qRotPos.w
        fmul ftemp
        fstp .qRotPos.w
        fstp .qRotPos.z
        fstp .qRotPos.y
        fstp .qRotPos.x
       
        fld ftemp
        fmul st(0),st(0)
        fstp l2       
    .endif
   
    ;make new (orthogonal) matrix from quaternion.
    ;rotPos.fromQuatL2(qRotPos, l2)
invoke Mat33_fromQuatL2,addr .rotPos,addr .qRotPos, l2

;calculate angular velocity with Eulers equations
    ;rotVel1.x = rotVel.x + (rotVel.y * rotVel.z * diagIyyMinusIzzOverIxx + rotForce.x * oneOverDiagIxx) * RKh
    fld  .rotVel.y
    fmul .rotVel.z
    fmul .diagIyyMinusIzzOverIxx
    fld  .rotForce.x
    fmul .oneOverDiagIxx
    fadd
    fmul RKh
    fadd .rotVel.x
    fstp .rotVel1.x
   
    ;rotVel1.y = rotVel.y + (rotVel.z * rotVel.x * diagIzzMinusIxxOverIyy + rotForce.y * oneOverDiagIyy) * RKh;
    fld  .rotVel.z
    fmul .rotVel.x
    fmul .diagIzzMinusIxxOverIyy
    fld  .rotForce.y
    fmul .oneOverDiagIyy
    fadd
    fmul RKh
    fadd .rotVel.y
    fstp .rotVel1.y
   
    ;rotVel1.z = rotVel.z + (rotVel.x * rotVel.y * diagIxxMinusIyyOverIzz + rotForce.z * oneOverDiagIzz) * RKh;
    fld  .rotVel.x
    fmul .rotVel.y
    fmul .diagIxxMinusIyyOverIzz
    fld  .rotForce.z
    fmul .oneOverDiagIzz
    fadd
    fmul RKh
    fadd .rotVel.z
    fstp .rotVel1.z

    ;rotVel = rotVel1
m2m .rotVel.x, .rotVel1.x
m2m .rotVel.y, .rotVel1.y
m2m .rotVel.z, .rotVel1.z
MethodEnd


Now I will show you the combine() method, which is EXTREMELY useful.
Usng this method, we can combine the mass properties of two or more Bodies (of any kind!!) into a single Body for the purposes of Physics updates.
Note that this does not include collision detection, we'll still need the geometric hulls of each component body , but we can apply the resulting forces to the composite 'parent' body.
So now, we can make an axe with a wooden handle and a metal head, and combine them into one, then throw it at someone we don't like :)

Method RigidBody.eulerStep,uses esi
LOCAL l2, ftemp

SetObject esi

    ;linPos += RKh * linVel
    ;linVel += linForce * oneOverMass * RKh   
    fld .linVel.x
    fld .linVel.y
    fld .linVel.z
    fld RKh
    fmul st(1),st(0)
    fmul st(2),st(0)
    fmul st(3),st(0)
    fUnload
    fadd .linPos.z
    fstp .linPos.z
    fadd .linPos.y
    fstp .linPos.y
    fadd .linPos.x
    fstp .linPos.x
   
    fld .linForce.x
    fld .linForce.y
    fld .linForce.z
    fld RKh
  fmul .oneOverMass
    fmul st(1),st(0)
    fmul st(2),st(0)
    fmul st(3),st(0)
    fUnload
    fadd .linVel.z
    fstp .linVel.z
    fadd .linVel.y
    fstp .linVel.y
    fadd .linVel.x
    fstp .linVel.x

    ;qRotPos.w -= RKh2 * (qRotPos0.x * rotVel.x + qRotPos0.y * rotVel.y + qRotPos0.z * rotVel.z);
    fld .qRotPos.w
    fld  .qRotPos0.x
    fmul .rotVel.x
    fld  .qRotPos0.y
    fmul .rotVel.y
    fadd
    fld  .qRotPos0.z
    fmul .rotVel.z
    fadd
    fmul RKh2
    fsub
    fstp .qRotPos.w
   
    ;qRotPos.x += RKh2 * (qRotPos0.w * rotVel.x - qRotPos0.z * rotVel.y + qRotPos0.y * rotVel.z);
    fld  .qRotPos0.w
    fmul .rotVel.x
    fld  .qRotPos0.z
    fmul .rotVel.y
    fsub
    fld  .qRotPos0.y
    fmul .rotVel.z
    fadd
    fmul RKh2
    fadd .qRotPos.x
    fstp .qRotPos.x
   
    ;qRotPos.y += RKh2 * (qRotPos0.z * rotVel.x + qRotPos0.w * rotVel.y - qRotPos0.x * rotVel.z);
    fld  .qRotPos0.z
    fmul .rotVel.x
    fld  .qRotPos0.w
    fmul .rotVel.y
    fadd
    fld  .qRotPos0.x
    fmul .rotVel.z
    fsub
    fmul RKh2
    fadd .qRotPos.y
    fstp .qRotPos.y

    ;qRotPos.z += RKh2 * (qRotPos0.x * rotVel.y - qRotPos0.y * rotVel.x + qRotPos0.w * rotVel.z);
    fld  .qRotPos0.x
    fmul .rotVel.y
    fld  .qRotPos0.y
    fmul .rotVel.x
    fsub
    fld  .qRotPos0.w
    fmul .rotVel.z
    fadd
    fmul RKh2
    fadd .qRotPos.z
    fstp .qRotPos.z

    ;l2 = qRotPos.x * qRotPos.x + qRotPos.y * qRotPos.y + qRotPos.z * qRotPos.z + qRotPos.w * qRotPos.w;
    fld .qRotPos.x
    fmul st(0),st(0)
    fld .qRotPos.y
    fmul st(0),st(0)   
    fld .qRotPos.z
    fmul st(0),st(0)
    fld .qRotPos.w
    fmul st(0),st(0)
fadd
fadd
fadd
fst l2 ;length of quaternion

fabs ;we want it to be 1.0 length (or really close)
fld1 ;so we'll check to see if we need to Normalize quaternion
fsub
fstp ftemp
fMin ftemp, fEpsilon
fstpReg eax
.if eax==ftemp   
  ; normalise to unit length (approximately)
        ;n = (l2 + 1.0) / (2.0 * l2);
        ;qRotPos.x *= n;
        ;qRotPos.y *= n;
        ;qRotPos.z *= n;
        ;qRotPos.w *= n;
        ;l2 *= n * n;
        fld l2
        fld1
        fadd
        fld l2
        fadd st(0),st(0)
        fdiv
        fstp ftemp
       
        fld .qRotPos.x
        fmul ftemp
        fld .qRotPos.y
        fmul ftemp
        fld .qRotPos.z
        fmul ftemp
        fld .qRotPos.w
        fmul ftemp
        fstp .qRotPos.w
        fstp .qRotPos.z
        fstp .qRotPos.y
        fstp .qRotPos.x
       
        fld ftemp
        fmul st(0),st(0)
        fstp l2       
    .endif
   
    ;make new (orthogonal) matrix from quaternion.
    ;rotPos.fromQuatL2(qRotPos, l2)
invoke Mat33_fromQuatL2,addr .rotPos,addr .qRotPos, l2

;calculate angular velocity with Eulers equations
    ;rotVel1.x = rotVel.x + (rotVel.y * rotVel.z * diagIyyMinusIzzOverIxx + rotForce.x * oneOverDiagIxx) * RKh
    fld  .rotVel.y
    fmul .rotVel.z
    fmul .diagIyyMinusIzzOverIxx
    fld  .rotForce.x
    fmul .oneOverDiagIxx
    fadd
    fmul RKh
    fadd .rotVel.x
    fstp .rotVel1.x
   
    ;rotVel1.y = rotVel.y + (rotVel.z * rotVel.x * diagIzzMinusIxxOverIyy + rotForce.y * oneOverDiagIyy) * RKh;
    fld  .rotVel.z
    fmul .rotVel.x
    fmul .diagIzzMinusIxxOverIyy
    fld  .rotForce.y
    fmul .oneOverDiagIyy
    fadd
    fmul RKh
    fadd .rotVel.y
    fstp .rotVel1.y
   
    ;rotVel1.z = rotVel.z + (rotVel.x * rotVel.y * diagIxxMinusIyyOverIzz + rotForce.z * oneOverDiagIzz) * RKh;
    fld  .rotVel.x
    fmul .rotVel.y
    fmul .diagIxxMinusIyyOverIzz
    fld  .rotForce.z
    fmul .oneOverDiagIzz
    fadd
    fmul RKh
    fadd .rotVel.z
    fstp .rotVel1.z

    ;rotVel = rotVel1
m2m .rotVel.x, .rotVel1.x
m2m .rotVel.y, .rotVel1.y
m2m .rotVel.z, .rotVel1.z
MethodEnd



We're almost ready to put it all together :)
Posted on 2008-01-26 20:53:20 by Homer
My first public demo sourcecode will support two kinds of SPRINGS, so it's worth taking some time to talk about the physics of Springs, and some of the things we might use them for.

Springs are a kind of CONSTRAINT.
Before I give equations for Springs, it's worth considering some even more primitive constraints.

The simplest constraint I can think of is called a "point constraint", whereby a point on entity A is forced to coincide with a point on entity B (and usually, vice-versa)... if A moves in space, then B is dragged along with it.. similarly, if we move B, then A is forced to move, so that "we do not violate the constraint".
What if we wanted to maintain a certain distance between those points ? We might call this a "rod constraint" - we can imagine a stiff rod that separates the points, and which may not be compressed or stretched.
We can calculate the distance between them fairy easily using pythagorus theorem .. we'd use a DotProduct.
Let's say that pA and pB are our two endpoints.
D = SquareRoot (pA dot pB)
We can also find the Normal Vector (direction) between the two points:
N = Normal (pB - pA)

The formula for a rod constraint might look something like this:
B = D * N
We are saying that "the new location of Point B is based on the Distance between A and B, multiplied by (in the Direction of) the Normal from A to B".

OK, if you can understand how that works, you are ready to introduce elasticity into the equation.
In fact, our Spring constraint will not directly determine a new Position, rather, it will calculate a Force which we can then introduce into our simulator, which will bear on the velocity, and thus affect the position.

The most simple spring equation looks like this:
F = -kD
where D is the vector difference in position between the two Points being constrained, and k is the Spring Constant which describes the "tightness" of the Spring.... larger values mean that the spring applies more force between the two endpoints, thus it will be "stiffer", and will require more force to stretch or compress it (and will apply more force to the endpoints)... smaller values wll make the spring more "elastic" and so it will require less force to stretch or compress the spring (and take longer to reach equilibrium).
The negative sign indicates that this Spring will attempt to bring these two endpoints together, rather than pushing them apart.. it will try to achieve a distance of zero, given enough time.

In the real world, springs are not perfect - they lose energy as heat, so they don't oscillate forever, they will eventually reach equilibrium. We need to introduce some Damping to simulate this energy loss, otherwise our Spring will oscillate forever (if there was no other forces).

So, our Spring equation becomes a little more complex:
F = - kD - bv
where b is the coefficient of damping and v is the relative velocity between the two points connected by the spring. Larger values for b increase the amount of damping so the object will come to rest more quickly, b=0 will allow the spring to oscillate forever, while negative values will actually accelerate the objects over time making your simulation explode...d'oh!

Generally, we don't want our Spring to bring the endpoints together, we want the Spring to maintain a given Distance between the endpoints... our formula becomes:
F = -k(D-d) * (N) - bv
where D is the CURRENT distance between the two points connected to the spring, d is the desired distance of separation, and N is the unit length direction vector between the two points: a to b, when applying the force to point a and vice versa.

Using this formula, we can implement all three kinds of constraints: Point, Rod and Spring, by setting the coefficients accordingly.

We can use Springs to make the Camera follow the Player, to simulate a Rope Bridge that sways and stretches as we walk across it, we can simulate Leaf Springs in a car, we can even use Springs to simulate SemiRigid (deformable) bodies such as a plate of jelly :)
The list of things you can do with Springs is almost endless, it just requires a little imagination :)

Springs are related to Point constraints, as we have seen - they apply force to the two endpoints.
If both endpoints belong to rigid bodies, we can take advantage of one of the laws of physics to apply the force to both bodies at once : "For every action, there is an equal and opposite reaction".
If we calculate the force applied to A, then the force applied to B is -F , and in the direction -N :)

But why stop at applying springs to Position only?
We can apply springs to acceleration, torque, or any other quantity that we wish to control over time.
Let's say we wish to bring an object from its current Velocity to its maximum Velocity over a given amount of Time... we can use a Spring to do that, easy!! This is an example of a Motor.

You should be starting to see just how useful Springs are to a physics simulation.

It is worth reiterating that Springs apply force to the endpoints, ie, the points at which they are attached.
For a rigid body, typically the endpoint will be on the surface of the body, NOT its center of mass.
Usually, this means that our linear spring force will cause both linear and angular forces to be applied to the body or bodies, because we will typically be applying our force "off-center"....

If you had a cube resting on a smooth surface, and you placed your finger in the center of one of its side faces, and gave it a push, it would slide along... but if you pushed it off-center, it would tend to want to Roll rather than slide.. yes?

Having calculated the Force to be applied to the attachment point of a Body, we should remember to apply the force AT THAT POINT, and NOT simply apply it to the center of mass.

In the next exciting instalment, I'll explain exactly how to do this, which by the way, will be very important later when we wish to resolve collisions :)


Posted on 2008-01-30 21:58:22 by Homer

So... how do we apply a linear Force to a Point on our Body?

I will assume that we know the current State of the Body, which importantly, includes its Angular Velocity.
Angular Velocity is a 3D Vector which describes how fast the Body is rotating, and in which Direction.
Most physics texts use the symbol W to describe angular velocity.
The length of the vector describes how fast the rotation is changing, typically in Radians per Second.
The direction of the Vector describes the 3D rotation axis... looking along that vector, the Body rotation is Clockwise.

Remembering our Linear equations, we know that a Body has some Mass, and that a Body which is moving linearly has some Linear Momentum.. it would require some Linear Force to change that momentum, and thus the speed and/or direction of motion.
The angular equivalent of Linear Force is called Torque, and the angular equivalent of Linear Momentum is called (suprise!) Angular Momentum.
A body that is rotating has some Angular Momentum - that is to say, it would require some Torque (rotational Force) to change the angular momentum, and thus to change the speed and/or direction of rotation.

Angular Velocity is generally considered in BodySpace rather than in WorldSpace.
The reason is due to the following formula:
T = R ◊ F
where :
T = torque (in BodySpace)
F = Force (in BodySpace) which is being applied to the Body
R = vector from centre of mass to the point on the Body where a Force is being applied.

Assume that we have a diagonalised Inertia Tensor. You may recall that it looks like this:

    Ixx 0 0
I = 0 Iyy 0
    0 0 Izz

We can calculate the new rotation matrix R from the existing rotation matrix r and the angular velocity W as follows:
R = r * Wb

Just remember that W is a vector, while r and R are rotation matrices or quaternions.

We can find out how a Torque force changes the Angular Velocity using Euler's Equations as follows:
dW.x = ((Iyy-Izz) / Ixx) * (W.y*W.z) + (T.x / Ixx)
dW.y = ((Izz-Ixx) / Iyy) * (W.z*W.x) + (T.y / Iyy)
dW.z = ((Ixx-Iyy) / Izz) * (W.x*W.y) + (T.z / Izz)
where dW is the Change in Angular Velocity due to the Torque, with respect to the Inertia Tensor.
Please do notice that angular velocity can change even if theres no Torque (T).

Having read all of that, you should be able to calculate the Torque due to an external Force, and thus the change in angular velocity (dW) due to torque.
Once we have that, we can calculate the NEW angular velocity by adding dW to the CURRENT angular velocity... and we can also find the NEW Orientation by multiplying the CURRENT Orientation with dW.

I've described how an input Force, when applied to a point on a Body, affects changes in orientation, but I have not shown how the input Force affects changes in linear momentum.
Expect me to update this post, I'm not really happy with it... it's hard to describe these equations without using visual aids such as diagrams, and a picture tells a thousand words.
Still, it's nice to see that Inertia Tensor finally being used for something!

Posted on 2008-01-31 00:45:24 by Homer

Forgive the rambling nature of the previous post.
This time I'll give a decent example, rather than a bunch of theory.

Let's suppose we wish to poke a Body with a stick - we wish to apply an external Force to some point on the surface of a Body.... and let's assume that we know the CURRENT state of the Body, and that from the current (linear and angular) Velocity, we have calculated the INTERNAL (linear) Force and (angular) Torque of the Body - that is to say, those Forces with respect to the Center of Mass (COM) which are products of its (linear and angular) momentum (aka Inertia). Let us also assume (for now) that the point of application of external force on the Body is given in WorldSpace, and so is the external Force.

So.. we know the current State, we know the internal forces, we have an external force F being applied at some point P on the Body, and both F and P are given in World coordinates.
We want to find the new internal forces after we applied the force, so we can then integrate those and thus find the new State of the Body. Where do we begin?

We begin by making the point P relative to the center of mass.
That's as simple as subtracting the position COM (vector) from the Point (vector).
We now have a vector, in worldspace, from COM to P, let's call it V.

Next, we find the CROSSPRODUCT of V and F.
The result is the "change in Torque" due to our applied force, given in WorldSpace, with respect to COM.
However, our internal torque (rotForce) is stored in diagonalised BodySpace (DSpace).
We stored it that way because even though our Body might be rotating in worldspace, it never rotates in bodyspace - this gives us a fixed reference coordinate system to work with, even though our world-relative coordinate system is rotating. So the next thing we need to do is transform the worldspace deltaTorque into diagonalised bodyspace. We can do that by multiplying it with the Transpose of the current rotation matrix.
Remember, our rotation matrix (rotPos) is used to transform from diagonalised bodyspace to worldspace.... we use it to orient the diagonalised bodyspace declaration of the body geometry.
If it rotates us from DSpace to WSpace, then its inverse (and transpose, since its a rot matrix) must transform us from WSpace back into DSpace, yes?

V = WorldPoint - COMposition
wspcTorque = CrossProduct (V,F)
m = Transpose(rotPos)
dspcTorque = m * F

In fact, rather than transposing the rotPos matrix, I have provided an EVEN FASTER function.
Its name is "Vec3_transMult".
It "premultiplies" a Vector by the transpose of a matrix, leaving the resulting Vec3 on the fpu stack.
Thus, dspcTorque = transMult (rotPos, F) , and we don't need to find m.
Finally, to find the NEW Torque, we simply add the result to our CURRENT Torque:
rotForce = rotForce + dspcTorque

That takes care of the rotational side of this exercise, but we haven't mentioned the linear side yet.
Surely, if we poke the Body with a stick, it doesn't JUST rotate, it probably moves linearly too, yes?
Yep, so how do we find the new linForce?
We keep linear force in WorldSpace coordinates, and the input Force is also in WorldSpace, so we just add them together:
linForce = linForce + F

Hangon, did we just apply the same force to our Body twice?
What about the law of conservation of momentum?
How is it that we are applying the same energy twice? Is this correct?
Yes, it is correct.
The linear momentum and the angular momentum are separate quantities, we calculate them separately.
We have NOT applied the same Force to the Body twice, we have applied the same Force to two completely separate components of the physical state.

At this point, we can calculate the new internal forces from the current internal forces and an external force. We know that our integrator will use those forces to update the body's other variables such as position. When we get to collision response, we'll be able to calculate the forces resulting from collisions then we can apply those forces to our Body or bodies, and let the integrator handle the rest.
Posted on 2008-01-31 02:31:21 by Homer
So - are we enjoying this rant so far?
Let me know your thoughts before I continue to collision detection/response stuff.
Posted on 2008-02-01 01:01:01 by Homer
yes! yes! :D We want more!
Posted on 2008-02-01 04:14:47 by HeLLoWorld
It's a little beyond my understanding at the moment Homer, but I do enjoy reading and trying to understand it. Keep up the good work.
Rags
Posted on 2008-02-01 13:16:38 by rags
I'm same as Rags.
Posted on 2008-02-01 13:35:46 by LocoDelAssembly
That is acceptable - to be honest, it's a little above me too, for I must translate this complex stuff into terms I can understand (my background in math is poor, I am mostly self-taught) ... and documenting my own understanding may help shed light on this stuff for you guys too (certainly, writing these notes in common language makes it more accessible than using greek notation !!) However, my motivation is also selfish to some degree, because the act of writing out my thoughts and perceptions etches them into my memory, and clarifies my own understand further... it's easy enough to understand any small detail, but seeing them put together into a context and understanding how the small pieces fit together to create something greater, THAT is enlightening.

I am putting together my first collision detection and response posts, and will provide them shortly.
To be honest, I am working on the code implementation of all this stuff as I go, and finding small issues to fix each day, which is the main reason I am yet to post any solid code - the code changes so often right now, it resembles a life form. But each day brings a small success, and any progress is good progress in my book.

Please take your time to read and re-read all the posts so far, and tell me if you struggle with anything in particular that I might perhaps have shed more light on (or explained better), totally forgot to mention, etc. Eventually this document will be enshrined in the form of an e-book and/or a dedicated wiki.


Posted on 2008-02-02 04:33:39 by Homer
Today I'd like to shed a little more light on this mysterious thing called an RK4 Integrator.
My example code will be VERY simple, its really just to help understand the process.
I will be showing position , velocity and acceleration just using single floats, so this could be considered a "one dimensional" example.

Euler integration is inaccurate because it is a linear solution being applied to a curved problem... Euler totally ignores this fact, and is only truly accurate when the timestep is infinitely small (ouch!)
RK4 is much more accurate than Euler integration, in fact its 5 orders more accurate.
Euler integration accumulates numerical error very quickly, and leads to unstable simulation.
RK4 is not our ONLY choice, but its a very nice choice, and not too difficult to implement...

For starters, let us describe what the Current State of our Body might look like:


State struct
position real4 ?
velocity real4 ?
derived Derivative <>
State ends


As you might recall, the (first) derivative of position is velocity, and the first derivative of velocity (second derivative of position) is acceleration.
Do not confuse DERIVED velocity with ACTUAL velocity.
Derived values are only temporary, intermediate values that we use to update our REAL ones.
This example stores the derived values in a struct also:

Derivative struct
derived_velocity      real4 ?
derived_acceleration real4 ?
Derivative ends


This example is using a VARIABLE TIMESTEP, which I have said before is a bad idea, but lets see it anyway.
The entrypoint to our RK4 code is here:

integrate proc pstate, ftimedelta
LOCAL _b:State
LOCAL _c:State
LOCAL _d:State
LOCAL halftime:real4

;Find the midpoint of the timestep
fld ftimedelta
fmul r4_Half
fstp halftime

invoke evaluateA,pstate
invoke evaluate, addr d_b, pstate, halftime
invoke evaluate, addr d_c, d_b, halftime
invoke evaluate, addr d_d, d_c, ftimedelta

;Now we calculate a kind of average of the four derived values,
;and add the resulting delta to our initial state values:

;dxdt = 1.0f/6.0f * (a.dx + 2.0f*(b.dx + c.dx) + d.dx)
;dvdt = 1.0f/6.0f * (a.dv + 2.0f*(b.dv + c.dv) + d.dv)
;state.x = state.x + dxdt*dt
;state.v = state.v + dvdt*dt

mov edx,pstate
fld  _b.derived.derived_velocity
fadd _c.derived.derived_velocity
fadd st(0),st(0)
fadd .State.derived.derived_velocity
fadd _d.derived.derived_velocity
fdiv r4_6
fadd .State.position
fstp .State.position

fld  _b.derived.derived_acceleration
fadd _c.derived.derived_acceleration
fadd st(0),st(0)
fadd .State.derived.derived_acceleration
fadd _d.derived.derived_acceleration
fdiv r4_6
fadd .State.velocity
fstp .State.velocity

ret

integrate endp


Now lets talk about whats happenening here before we look at any more code.
RK4 works by calculating the derivatives at the start of the timestep, halfway through, and at the end of the timestep, and then it averages out the error over the timestep, and finally updates the state values.
The first thing we see is some code to find "half the timestep".
Next, we see a call to "evaluateA".
This function calculates the derivates of the given State struct at the start of the timestep, and stores them inside there.
Then we see three calls to "evaluate".
This function takes an input state's derivates and calculates new derivatives at a given Time.
Each set of derivatives is used to forge a new State struct, each feeding into the next stage.
Finally, we see the code which updates our State based on all those derivatives that we found.


;Calculate the Derivatives at the start of the timestep..
;We will store these in the given State structure
evaluateA proc pState
mov edx,pState
;derived velocity = initial velocity
m2m .Derived.derived_velocity, .State.velocity
;derived acceleration = initial acceleration
acceleration .State
fstp .Derived.derived_acceleration
ret
evaluate endp


That one is pretty simple - we only call it once, to calculate State's derived values.


evaluate proc pStateOut,pStateIn, ftimedelta
LOCAL state:State
mov edx,pStateOut
mov eax,pStateIn
;output position = input position + (derived_velocity * timedelta)
fld .State.derived.derived_velocity
fmul ftimedelta
fadd .State.position
fstp .State.position
;output velocity = input velocity + (derived_acceleration * timedelta)
fld .State.derived.derived_acceleration
fmul ftimedelta
fadd .State.velocity
fstp .State.velocity
;Now calculate the derived values for the new state values
invoke evaluateA,pStateOut
ret
evaluate endp


Slightly more complex - we calculate new state values via given derivatives, and then we hand the new state to evaluateA to find ITS derivatives.


.data
fk real4 10.0f
fb real4 1.0f
.code
acceleration macro state
;This demo macro implements a spring with damping
;returns a value on the fpu stack
; - k*state.x - b*state.v
fld fk
fmul state.position
fld fb
fmul state.velocity
fsub
endm


This macro implements a Spring and Damper using Hooke's spring formula.
We can put anything we want here, we could find acceleration due to forces, or whatever.
This is just an example showing how to CORRECTLY implement Runge-Kutta fourth order numerical integration.

I guess the most important things to take home from all of this are:
A) Don't screw with your current state values during the integration stages - wait until the end
B) Derivatives are "delta values" that tell us how much some value changes - which is why we can't screw with our current state until we have blended the four derivatives and obtained our final delta values.
C) RK4 works by sampling the derivatives four times, enabling us to average out the error over the timestep.

Did this post help you to understand RK4 implementation?
Posted on 2008-02-06 09:46:23 by Homer