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

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

vertices [ tris

*[1] ],*

vertices [ trisvertices [ 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;

}

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.

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.

*
*

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

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!

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.

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.

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.

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

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

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

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

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.

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.

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

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

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

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

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.

Regards,

Biterider

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

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

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

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

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

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

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

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

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!

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.

So - are we enjoying this rant so far?

Let me know your thoughts before I continue to collision detection/response stuff.

Let me know your thoughts before I continue to collision detection/response stuff.

yes! yes! :D We want more!

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

Rags

I'm same as Rags.

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.

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.

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?

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?