Here's an idea of what the table of friction coefficients might look like (and sorry about the formatting)... each cell contains a pair of values... static and dynamic friction coefficients for given pair of materials... or just 0 for "unknown" - this is just me starting to collate some useful data, wanna help fill some of these fields?
Slippery conditions = something slippery is lubricating the surface(s), eg oil or water
Sticky conditions = something like race tyres on concrete
Slippery conditions = something slippery is lubricating the surface(s), eg oil or water
Sticky conditions = something like race tyres on concrete
.data
MyTableElement struct
_Static real4 ?
_Dynamic real4 ?
MyTableElement ends
MyTableRow struct
Wood MyTableElement <>
Stone MyTableElement <>
Ice MyTableElement <>
Glass MyTableElement <>
Metal MyTableElement <>
Rubber MyTableElement <>
Velcro MyTableElement <>
Meat MyTableElement <>
MyTableRow ends
;--------------------------------------------------------------------------------------------------------------------------------------
; WOOD STONE ICE GLASS METAL RUBBER VELCRO MEAT
;--------------------------------------------------------------------------------------------------------------------------------------
;-------------------
;NORMAL CONDITIONS
;-------------------
;WOOD
MyTableRow {<0.0f,0.0f>, <0.5f,0.4f>, <0.2f,0.1f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;STONE
MyTableRow {<0.5f,0.4f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <1.0f,0.8f>, <0.0f,0.0f>, <0.0f,0.0f>}
;ICE
MyTableRow {<0.2f,0.1f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.1f,0.03f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;GLASS
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.1f,0.03f>, <0.95f,0.4f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;METAL
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.6f,0.4f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;RUBBER
MyTableRow {<0.0f,0.0f>, <1.0f, 0.8f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;VELCRO
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <6.0f,4.0f>, <0.0f,0.0f>}
;MEAT
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;-------------------
;STICKY CONDITIONS
;-------------------
;WOOD
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;STONE
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <1.5f,1.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;ICE
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;GLASS
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;METAL
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;RUBBER
MyTableRow {<0.0f,0.0f>, <1.5f,1.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;VELCRO
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;MEAT
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;---------------------
;SLIPPERY CONDITIONS
;---------------------
;WOOD
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;STONE
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.7f,0.5f>, <0.0f,0.0f>, <0.0f,0.0f>}
;ICE
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;GLASS
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;METAL
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.1f,0.05f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;RUBBER
MyTableRow {<0.0f,0.0f>, <0.7f,0.5f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;VELCRO
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
;MEAT
MyTableRow {<0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>, <0.0f,0.0f>}
.code
Here's a pretty picture of a collision thats about to occur.
Its showing some vectors with respect to the Center of Mass, but in fact we'll be interested in these vectors with respect to each Point of Collision.
The green line indicates the closing velocity of the Body.
The red line indicates the component of the closing velocity which is perpendicular with the collision normal.... our coefficient of restitution plays against the Red Line.
The yellow line indicates the component of the closing velocity which is tangent to the collision normal.... our coefficients of friction play against the Yellow Line.
I just wanted to visually show you what these vectors are because we'll be using them.
RELATIVE VELOCITY
If our Body is colliding with some unmovable scenery in the World (such as our World Bounding Planes), then the Relative Velocity of the Collision is simply that of the Body.
RelativeVelocity = BodyA.CMVelocity
But if our Body is colliding with another Body, then the Relative Velocity is given as:
RelativeVelocity = BodyA.CMVelocity - BodyB.CMVelocity
COLLISION NORMAL
If our Body is colliding with some unmovable scenery in the World (such as our World Bounding Planes) then the Normal is that of the collision surface.. ie for collision with a flat floor plane, the Collision Normal would be <0.0, 1.0, 0.0>
CollisionNormal = SurfaceNormal
But if our Body is colliding with another Body, then the Collision Normal is taken from the relative velocities of the Bodies (at center of mass), and by convention, it is with respect to Body A:
CollisionNormal = Normalize (RelativeVelocity )
Note that the Collision Normal is a NORMAL (has a length of 1.0)
CLOSING VELOCITY
With the correct Collision Normal in hand, the closing velocity is given as:
ClosingVelocity = DotProduct (RelativeVelocity , CollisionNormal)
That works regardless of what kind of collision we have.
Note that this scalar value is with respect to the Center(s) of Mass, and does not tell us enough information alone to resolve the collision - it does not take rotation into account ("angular effects"). But its a good start.
In the next post, we'll begin to look at how we resolve a collision at the Particle level.
Then we'll expand on that to handle RigidBody collisions.
If our Body is colliding with some unmovable scenery in the World (such as our World Bounding Planes), then the Relative Velocity of the Collision is simply that of the Body.
RelativeVelocity = BodyA.CMVelocity
But if our Body is colliding with another Body, then the Relative Velocity is given as:
RelativeVelocity = BodyA.CMVelocity - BodyB.CMVelocity
COLLISION NORMAL
If our Body is colliding with some unmovable scenery in the World (such as our World Bounding Planes) then the Normal is that of the collision surface.. ie for collision with a flat floor plane, the Collision Normal would be <0.0, 1.0, 0.0>
CollisionNormal = SurfaceNormal
But if our Body is colliding with another Body, then the Collision Normal is taken from the relative velocities of the Bodies (at center of mass), and by convention, it is with respect to Body A:
CollisionNormal = Normalize (RelativeVelocity )
Note that the Collision Normal is a NORMAL (has a length of 1.0)
CLOSING VELOCITY
With the correct Collision Normal in hand, the closing velocity is given as:
ClosingVelocity = DotProduct (RelativeVelocity , CollisionNormal)
That works regardless of what kind of collision we have.
Note that this scalar value is with respect to the Center(s) of Mass, and does not tell us enough information alone to resolve the collision - it does not take rotation into account ("angular effects"). But its a good start.
In the next post, we'll begin to look at how we resolve a collision at the Particle level.
Then we'll expand on that to handle RigidBody collisions.
Let us consider a single Point-collision between two Bodies (or a Body and a fixed surface).
We are handed the WorldSpace coordinate of the point of collision, and we are able to transform WorldSpace coordinates into any given BodySpace.
At the moment of impact, we are certain that if we transform the collision-point into the space of any Body involved in the collision, that Point will rest apon the Bounding Hull of that Body.
In other words, for a Body-Body collision, we can find the BodySpace coordinate of the collision point for EACH body's space.
I'm trying to reinforce something I mentioned earlier - that a Point collision actually involves TWO points - one on each of the colliding entities that happen to meet when the collision occurs.
These Points are the places on each entity where the collision-response Impulse should be applied :)
So we can begin to describe a general-purpose method for resolving point-based collisions...
We are handed the WorldSpace coordinate of the point of collision, and we are able to transform WorldSpace coordinates into any given BodySpace.
At the moment of impact, we are certain that if we transform the collision-point into the space of any Body involved in the collision, that Point will rest apon the Bounding Hull of that Body.
In other words, for a Body-Body collision, we can find the BodySpace coordinate of the collision point for EACH body's space.
I'm trying to reinforce something I mentioned earlier - that a Point collision actually involves TWO points - one on each of the colliding entities that happen to meet when the collision occurs.
These Points are the places on each entity where the collision-response Impulse should be applied :)
So we can begin to describe a general-purpose method for resolving point-based collisions...
Here's an example function which resolves the collision of two PARTICLES.
Particles don't have any Orientation, so this code does NOT handle 'Angular Effects'.
Because of this, we will *NOT* be using this function.
It's simply an example.
Remember that ALL collisions can be expressed in terms of Particle collisions, so the solution for a Rigid Body is sure to be quite similar to this :)
Next we'll expand on this to handle angular effects for rigid bodies, and then we'll add isotropic friction.
Particles don't have any Orientation, so this code does NOT handle 'Angular Effects'.
Because of this, we will *NOT* be using this function.
It's simply an example.
Remember that ALL collisions can be expressed in terms of Particle collisions, so the solution for a Rigid Body is sure to be quite similar to this :)
Method Particle.resolveVelocity,uses esi,pOtherParticle, pvContactNormal, duration:real8
LOCAL separatingVelocity:real8,newSepVelocity:real8,restitution:real8, accCausedSepVelocity:real8, impulse:real8
LOCAL deltaVelocity:real8,ftemp:real8
LOCAL accCausedVelocity:Vec3, impulsePerIMass:Vec3
SetObject esi
; Find the velocity in the direction of the contact.
OCall esi.calculateSeparatingVelocity, pOtherParticle
fstp separatingVelocity
; Check whether it needs to be resolved.
.if separatingVelocity > 0
; The contact is either separating or stationary - there’s no impulse required.
ExitMethod
.endif
OCall esi.calculateRestitution, pOtherBody
fstp restitution
; Calculate the new separating velocity.
; newSepVelocity = -separatingVelocity * restitution;
fld separatingVelocity
fchs
fmul restitution
fstp newSepVelocity
; Check the velocity build-up due to acceleration only.
OCall esi.getLastFrameAcceleration
__StowFloat3 accCausedVelocity
.if pOtherParticle!=0
OCall pOtherBody::Particle.getLastFrameAcceleration
__SubFromFloat3 accCausedVelocity, accCausedVelocity
.endif
;accCausedSepVelocity = accCausedVelocity * contactNormal * duration
mov edx,pvContactNormal
DotProduct accCausedVelocity, .Vec3
fmul duration
fstp accCausedVelocity
; If we’ve got a closing velocity due to acceleration build-up,
; remove it from the new separating velocity.
.if accCausedSepVelocity < 0
;newSepVelocity += restitution * accCausedSepVelocity;
fld accCausedSepVelocity
fmul restitution
fadd newSepVelocity
fstp newSepVelocity
; Make sure we haven’t removed more than was there to remove.
.if (newSepVelocity < 0)
fmov newSepVelocity , 0.0
.endif
.endif
;deltaVelocity = newSepVelocity - separatingVelocity
fld newSepVelocity
fsub separatingVelocity
fstp deltaVelocity
; We apply the change in velocity to each object in proportion to
; its inverse mass (i.e., those with lower inverse mass get less change in velocity).
fld .inverseMass
.if pOtherParticle!=0
mov edx,pOtherParticle
fadd .Particle.inverseMass
totalInverseMass += particle[1]->getInverseMass();
.endif
fstp totalInverseMass
; If all particles have infinite mass, then impulses have no effect.
.if totalInverseMass <= 0
ExitMethod
.endif
; Calculate the impulse to apply.
;impulse = deltaVelocity / totalInverseMass;
fld deltaVelocity
fdiv totalInverseMass
fstp impulse
; Find the amount of impulse per unit of inverse mass.
; impulsePerIMass = contactNormal * impulse
mov edx,pvContactNormal
CrossProduct .Vec3, impulse
__StowFloat3 impulsePerIMass
; Apply impulses in the direction of the contact, and proportional to the inverse mass.
;particle.velocity += impulsePerIMass * particle.inverseMass
__ScaleFloat3 impulsePerIMass, .inverseMass
__AddToFloat3 .Velocity, .Velocity
.if pOtherParticle!=0
; Particle 1 goes in the opposite direction - note how we flip the sign
;particle.velocity += impulsePerIMass * -particle.inverseMass
mov edx,pOtherParticle
fld .Particle.inverseMass
fchs
fstp ftemp
__ScaleFloat3 impulsePerIMass, ftemp
__AddToFloat3 .Particle.Velocity, .Particle.Velocity
.endif
MethodEnd
Next we'll expand on this to handle angular effects for rigid bodies, and then we'll add isotropic friction.
Before I go on, I'd like to talk about Contact Space.
Some of the math problems we're going to face (especially friction) can be rather difficult to solve if we attempt to solve them in the spatial systems we're familiar with (worldspace, bodyspace).... but can easily be solved if we move the problem into CONTACT space.
In this spatial coordinate system, one of the Axes (say, X) will be our Contact Normal.
The other two othogonal axes (Y, Z) are arbitrary - we'll look at one way to choose them.
In order to transform coordinates from worldspace to Contact space, we'll build a 3x3 transformation matrix. Since this matrix is actually a pure rotation matrix, we can use the Transpose Trick to transform coordinates back the other way (contact-to-world).
Now I'm going to show you a NEW TRICK.
This is a really efficient way to build a Rotation Matrix, given just one Vector, and the assumption that this Vector will always be the X Axis in the new coordinate system.
The 3x3 matrix we're building can be thought of as a set of 3 column-vectors.
Given a vector with components abc, the matrix will look like this:
a d g
b e h
c f i
So we'll need to find two vectors (def and ghi) which are orthogonal to abc and each other.
Orthogonal means that they're 90 degrees apart.
The most common ways to find these vectors involve finding the major and minor axes used by our input vector, then performing some cross-products.
I'm going to show you a cheating way which just makes the above a stupid idea.
Imagine if we have a 2D (XY) set of axes, if we swap the X and Y values, we just rotated the system by 90 degrees - yes?
The same is true in a 3D system - except that we ROTATE the axes.
abc = acb
def = bac
ghi = cba
So all we have to do is write the same vector component values out into each Column of our matrix, rotating the components as we go.
If the input vector was (0.0,0.86,0.5) , then the matrix would be:
0.0 0.5 0.86
0.86 0.0 0.5
0.5 0.86 0.0
We'll shove our Contact Normal in the first column, and fill out the rest as described.
Once we've shoved those orthogonal axis vectors in the two remaining columns, we're done, thats our transform matrix - how we FOUND these orthogonal axes is NOT RELEVANT - if we can find them "as a Given" and with no cost - then why shouldn't we use them?
Perhaps this solution isnt quite right - maybe we need to also toggle the Sign in places - who can contribute to solve this rather simple seeming problem?
Actually, I'm SURE that some of the signs are wrong, because that looks suspiciously like a General Rotation Matrix to me !!
However - we'll be transforming data into and out of the coordinate system described by our matrix - its only being used as a TRANSIENT space - we don't care about the DIRECTION of the two discovered axes - so the SIGN issue really is not an issue at all :)
Some of the math problems we're going to face (especially friction) can be rather difficult to solve if we attempt to solve them in the spatial systems we're familiar with (worldspace, bodyspace).... but can easily be solved if we move the problem into CONTACT space.
In this spatial coordinate system, one of the Axes (say, X) will be our Contact Normal.
The other two othogonal axes (Y, Z) are arbitrary - we'll look at one way to choose them.
In order to transform coordinates from worldspace to Contact space, we'll build a 3x3 transformation matrix. Since this matrix is actually a pure rotation matrix, we can use the Transpose Trick to transform coordinates back the other way (contact-to-world).
Now I'm going to show you a NEW TRICK.
This is a really efficient way to build a Rotation Matrix, given just one Vector, and the assumption that this Vector will always be the X Axis in the new coordinate system.
The 3x3 matrix we're building can be thought of as a set of 3 column-vectors.
Given a vector with components abc, the matrix will look like this:
a d g
b e h
c f i
So we'll need to find two vectors (def and ghi) which are orthogonal to abc and each other.
Orthogonal means that they're 90 degrees apart.
The most common ways to find these vectors involve finding the major and minor axes used by our input vector, then performing some cross-products.
I'm going to show you a cheating way which just makes the above a stupid idea.
Imagine if we have a 2D (XY) set of axes, if we swap the X and Y values, we just rotated the system by 90 degrees - yes?
The same is true in a 3D system - except that we ROTATE the axes.
abc = acb
def = bac
ghi = cba
So all we have to do is write the same vector component values out into each Column of our matrix, rotating the components as we go.
If the input vector was (0.0,0.86,0.5) , then the matrix would be:
0.0 0.5 0.86
0.86 0.0 0.5
0.5 0.86 0.0
We'll shove our Contact Normal in the first column, and fill out the rest as described.
Once we've shoved those orthogonal axis vectors in the two remaining columns, we're done, thats our transform matrix - how we FOUND these orthogonal axes is NOT RELEVANT - if we can find them "as a Given" and with no cost - then why shouldn't we use them?
Perhaps this solution isnt quite right - maybe we need to also toggle the Sign in places - who can contribute to solve this rather simple seeming problem?
Actually, I'm SURE that some of the signs are wrong, because that looks suspiciously like a General Rotation Matrix to me !!
However - we'll be transforming data into and out of the coordinate system described by our matrix - its only being used as a TRANSIENT space - we don't care about the DIRECTION of the two discovered axes - so the SIGN issue really is not an issue at all :)
Anyway, just in case my little cheat is insufficient / Direction is important to us, the following macro was added to the Handy Math include (attached previously).
It creates a rotation matrix that transforms coordinates from 'Contact Space' into WorldSpace.
Since its a pure Rotation matrix, we can use the jolly Transpose trick to transform from WorldSpace to ContactSpace ("TransMult" function)...
;Creates an Orthogonal set of Normal Vectors (axes) from a given Vector, as Columns of a Mat33
;DO NOT USE EDX
Mat33_OrthoNormalBasis macro Mat,V
;The X column vector is taken directly from our collision Normal
m2m Mat.m00, V.x
m2m Mat.m10, V.y
m2m Mat.m20, V.z
;Pick an axis which is orthogonal to the Major axis
fAbsMax V.x, V.y
fstpReg edx
.if edx==V.x
fld Mat.m00
fchs
fld Mat.m20
;s = 1 /sqrt(m20*m20 + m00*m00)
; The new X-axis is at right angles to the world Y-axis
; m01 = m20*s
; m11 = 0
; m21 = m00*s
; The new Y-axis is at right angles to the new X- and Z- axes
; m02 = m10*m01
; m12 = m20*m01 - m00*m21
; m22 = -m10*m01
fld1
fld Mat.m20
fmul st(0),st(0)
fld Mat.m00
fmul st(0),st(0)
fadd
fsqrt
fdiv
fmul st(2),st(0)
fmul
mov Mat.m11,0
fstp Mat.m01
fstp Mat.m21
fld Mat.m10
fmul Mat.m01
fstp Mat.m02
fld Mat.m20
fmul Mat.m01
fld Mat.m00
fmul Mat.m21
fsub
fstp Mat.m12
fld Mat.m10
fmul Mat.m01
fchs
fstp Mat.m22
.else
fld Mat.m10
fld Mat.m20
fchs
;s = 1/sqrt(m20*m20 + m10*m10)
; The new X-axis is at right angles to the world X-axis
; m01 = 0
; m11 = -m20*s
; m21 = m10*s
; The new Y-axis is at right angles to the new X- and Z- axes
;m02 = m10*m21 - m20*m11
;m12 = -m00*m21
;m22 = m00*m11
fld1
fld Mat.m20
fmul st(0),st(0)
fld Mat.m10
fmul st(0),st(0)
fadd
fsqrt
fdiv
;
fmul st(2),st(0)
fmul
;
mov Mat.m01,0
fstp Mat.m11
fstp Mat.m21
fld Mat.m10
fmul Mat.m21
fld Mat.m20
fmul Mat.m11
fsub
fstp Mat.m02
fld Mat.m00
fmul Mat.m21
fchs
fstp Mat.m12
fld Mat.m00
fmul Mat.m11
fstp Mat.m22
.endif
endm
Now that we're able to toss our collision Normal at a macro and spit out a matrix, we're ready to go on.
Our goal is to calculate the impulse at the collision, right?
And from there, we'd like to be able to calculate the change in velocity of each object, given that impulse.
For the moment, we're NOT considering Friction - and for Frictionless collisions, the impulses generated at the contact are only applied along the collision normal.
Read that again - the IMPULSE is applied ONLY along the collision normal.
We’d like to end up with a simple number that tells us the change in velocity AT THE CONTACT POINT, in the direction of the contact normal, for each unit of impulse, something like a SIGNED SCALAR (signed float used as multiplier) would be nice, yes?
It's important to realize that the linear and angular stuff in our Bodies is separate stuff. We have to treat them separately, and then combine them at the end.
So lets start with the linear component.
The linear component is very simple, noting that this is ONLY for the Linear component
The linear change in velocity for a unit impulse will be in the direction of the impulse, with a magnitude given by the inverse mass:
DeltaVelocity * Impulse = inverseMass
If theres two Bodies involved, inverseMass will be the Sum of their masses (add them).
The angular stuff is a bit more tricky, we have to do it in three parts.
torquePerUnitImpulse =CrossProduct (relativeContactPosition , contactNormal)
rotationPerUnitImpulse =inverseInertiaTensor * torquePerUnitImpulse
velocityPerUnitImpulse =CrossProduct (rotationPerUnitImpulse , relativeContactPosition)
The first equation tells us the amount of impulsive torque generated from a unit of impulse.
The second one tells us the change in angular velocity for a unit of impulsive torque.
And the third tells us the velocity at the contact point which is due only to the body rotating...
It is a velocity in world space. When it comes to implementing Friction, we'll need to transform this Worldspace vector into contact coordinates. This would give us a vector of velocities that a unit impulse would cause. Note that since we DONT have Friction, we're only interested in the velocity in the direction of the contact normal.
In contact coordinates this is the X axis, so our value is the x component of the resulting vector:
velocityPerUnitImpulseContact = Mat33_transMult (ContactToWorld,velocityPerUnitImpulse)
angularComponent = velocityPerUnitImpulseContact.x
Of course we COULD do it that way.
But we already know a faster way to find the scalar component of one Vector along another:
angularComponent = DotProduct (velocityPerUnitImpulse , contactNormal)
For frictionless collisions, the result will be the same, so for frictionless collisions,we don't actually NEED to use the ContactSpace matrix.
But I wanted to show it to you now so that it will 'click' in your mind when we do get around to adding Friction.
For each object in the collision, we're now able to calculate the change in both Linear and Angular velocity: at the contact point, and PER UNIT impulse. We now add those velocity values together to get one overall value (even if theres two bodies) which describes the total change in velocity at the collision point.
We now know the closing velocity at the contact point.
The desired separating velocity is the negative of that.
Since we now know the desired separating velocity, we can now calculate an impulse which would produce that velocity change.
Then we just need to Apply it..
It's time to start putting together everything we've learned :)
Setting up stuff that we'll need to resolve the collision... pretty much everything we do will be with respect to the Contact point... we're solving each point-collision with respect to BOTH bodies (if theres two), which reduces the amount of calculations overall.
1. The first thing we do is set up our Contact-to-World transform.
2. Then we calculate the position of the contact point, relative to the Body or Bodies.
We also compute the average Coefficient of Restitution for the Body or Bodies.
3. Next, we calculate the absolute velocity of the contact point, relative to the Bodies, and in Contact coords.
4. And then we calculate the magnitude of the change in velocity due solely to acceleration during this Frame along the collision normal.
5. Finally, we calculate the change in velocity we'd LIKE to see along the collision normal.
This, combined with Step 4, will allow us to SCALE the output to exactly what we'd LIKE to see.
Stay tuned for the rest of this method!
;This is the entrypoint for collision response.
Method CollisionHull.CalculateResponse, uses esi, pTargetConfigA,pTargetConfigB,pvContactPoint,pvContactNormal, pBodyB, bWantFriction:BOOL
LOCAL relativeContactPositionA:Vec3
LOCAL relativeContactPositionB:Vec3
LOCAL contactVelocity:Vec3
LOCAL desiredDeltaVelocity:real8, restitution:real8
LOCAL velocityFromAcc:real4
LOCAL impulsiveTorque:Vec3,impulse:Vec3 ;,rotationChange:Vec3
LOCAL ContactToWorld:Mat33
SetObject esi
; Calculate a set of axes at the contact point.
mov eax,pvContactNormal
Mat33_OrthoNormalBasis ContactToWorld, .Vec3
; Find the relative position of the contact
mov eax,pvContactPoint
mov edx,pTargetConfigA
__SubFloat3 .Vec3, .configuration.CMPosition
__StowFloat3 relativeContactPositionA
fld .CoefficientOfRestitution
.if pBodyB!=0
mov eax,pvContactPoint
mov edx,pTargetConfigB
__SubFloat3 .Vec3, .configuration.CMPosition
__StowFloat3 relativeContactPositionB
mov edx,pBodyB
fadd .CollisionHull.CoefficientOfRestitution
fmul r4_Half
.endif
fstp restitution
; Find the relative velocity at the contact point.
OCall esi.calculateLocalVelocity, pTargetConfigA,addr relativeContactPositionA, addr ContactToWorld
__StowFloat3 contactVelocity
.if pBodyB!=0
OCall pBodyB::CollisionHull.calculateLocalVelocity, pTargetConfigB, addr relativeContactPositionB, addr ContactToWorld
__SubFromFloat3 contactVelocity, contactVelocity
.endif
; Calculate the acceleration-induced velocity accumulated this frame
mov velocityFromAcc,0
.if .bIsAwake==TRUE
mov eax,pTargetConfigA
mov edx,pvContactNormal
DotProduct .Vec3, .configuration.CMVelocityChange
fstp velocityFromAcc
.endif
mov edx,pBodyB
.if edx!=0 && .CollisionHull.bIsAwake==TRUE
mov eax,pTargetConfigB
mov edx,pvContactNormal
DotProduct .Vec3, .configuration.CMVelocityChange
fchs
fadd velocityFromAcc
fstp velocityFromAcc
.endif
velocityLimit equ r4_Quarter
; Calculate the desired change in velocity for resolution
; desiredDeltaVelocity = -contactVelocity.x -thisRestitution * (contactVelocity.x - velocityFromAcc)
; If the velocity is very slow, limit the restitution
fld contactVelocity.x
fchs
fld contactVelocity.x
fabs
fsub velocityLimit
fstpReg eax
.ifBitSet eax,BIT31 ;if abs(contactVelocity.x) < velocityLimit
;thisRestitution = 0
.else
fsub restitution
.endif
fld contactVelocity.x
fsub velocityFromAcc
fmul
fstp desiredDeltaVelocity
Setting up stuff that we'll need to resolve the collision... pretty much everything we do will be with respect to the Contact point... we're solving each point-collision with respect to BOTH bodies (if theres two), which reduces the amount of calculations overall.
1. The first thing we do is set up our Contact-to-World transform.
2. Then we calculate the position of the contact point, relative to the Body or Bodies.
We also compute the average Coefficient of Restitution for the Body or Bodies.
3. Next, we calculate the absolute velocity of the contact point, relative to the Bodies, and in Contact coords.
4. And then we calculate the magnitude of the change in velocity due solely to acceleration during this Frame along the collision normal.
5. Finally, we calculate the change in velocity we'd LIKE to see along the collision normal.
This, combined with Step 4, will allow us to SCALE the output to exactly what we'd LIKE to see.
Stay tuned for the rest of this method!
Here's the rest of that method.
1. We calculate the Impulse using either with or without Friction.
2. The impulse we calculated is in Contact coords, so we convert it to World coords.
3. We split the impulse into Linear and Angular components, and apply them to the Body(s).
As you can see, I'm not applying them directly - I'm accumulating them so that each Body has the average effect of N impulses applied to it simultaneously - this isnt strictly correct, the real problem is a simultaneous equation, and the real solution involves 'Jacobians', something I'm not too familiar with... but the average of the change in momenta due to N impulses is pretty close and looks believable at a fraction of the cost of the Jacobian solution.
Although theres a lot going on here, the really interesting stuff is buried out of sight.. Now I suppose you want to see the new methods whose names were exposed in that source?
We'll do that in the next post :)
PS : Attached is an update of the Handy Math includefile, which contains some new stuff including the __NegScaleFloat3 macro, as well as 90% of the other odd-looking macros you've seen me using (the remainder can be found in OA32's fMath includefile)
ONLY REGISTERED MEMBERS CAN SEE DOWNLOAD LINKS.
; Calculate impulse in Contact coords
.if bWantFriction==FALSE
OCall esi.CalculateFrictionlessImpulse, addr impulse, pvContactNormal, addr ContactToWorld, addr relativeContactPositionA, addr relativeContactPositionB, pTargetConfigA, pBodyB, pTargetConfigB
.else
;Look up the Friction Coefficients based on the Materials of both Bodies
mov eax,sizeof MyTableElement
mul .dMaterialType
.if pBodyB!=0
mov edx,pBodyB
push eax
mov eax,sizeof MyTableRow
mul .CollisionHull.dMaterialType
pop edx
add eax,edx
.endif
lea edx,FrictionTable
add eax,edx
OCall esi.CalculateFrictionImpulse, addr impulse, addr ContactToWorld, addr contactVelocity,addr relativeContactPositionA,addr relativeContactPositionB,pTargetConfigA,pBodyB,pTargetConfigB, eax
.endif
; Convert impulse from Contact to World coords
Vec3_mult_Mat33 ContactToWorld, impulse
__StowFloat3 impulse
; Split the impulse into linear and angular components,
; and apply them to the Body(s)
CrossProduct relativeContactPositionA, impulse
__StowFloat3 impulsiveTorque
mov eax,pTargetConfigA
; This is how we would convert ImpulseTorque into Delta-Angular-Velocity
; BUT WE WILL NOT BOTHER because this Simulator loves Angular Momentum
; and so it will extract the Angular Velocity directly from Angular Momentum
; Vec3_mult_Mat33 .configuration.InverseWorldInertiaTensor, impulsiveTorque
; __StowFloat3 rotationChange
; Here we're getting linear acceleration (delta-velocity) from the impulse by removing Mass
__ScaleFloat3 impulse, .OneOverMass
__AddToFloat3 .vAccumulated_Delta_Velocity, .vAccumulated_Delta_Velocity
; Here we're just collecting the change in angular momentum due to Torque
__LoadFloat3 impulsiveTorque
__AddToFloat3 .vAccumulated_Delta_AngMomentum, .vAccumulated_Delta_AngMomentum
inc .dNumSimultaneousCollisions
.if pBodyB!=0
; Work out body one's linear and angular changes
; NOTE :: Crossed members have been SWITCHED
; We have FLIPPED THE SIGN - we want to apply the equal, opposite impulse
CrossProduct impulse, relativeContactPositionB
__StowFloat3 impulsiveTorque
; This is how we would convert ImpulseTorque into Delta-Angular-Velocity
; BUT WE WILL NOT BOTHER because this Simulator loves Angular Momentum
; and so it will extract the Angular Velocity directly from Angular Momentum
; mov eax,pTargetConfigB
; Vec3_mult_Mat33 .configuration.InverseWorldInertiaTensor, impulsiveTorque
; __StowFloat3 rotationChange
; Here we're getting linear acceleration (delta-velocity) from the impulse by removing Mass
; Note that this time we have FLIPPED THE SIGN - we want to apply the equal, opposite impulse
mov edx,pBodyB
__NegScaleFloat3 impulse, .CollisionHull.OneOverMass
__AddToFloat3 .CollisionHull.vAccumulated_Delta_Velocity, .CollisionHull.vAccumulated_Delta_Velocity
; Collect ye olde momentum
__LoadFloat3 impulsiveTorque
__AddToFloat3 .CollisionHull.vAccumulated_Delta_AngMomentum, .CollisionHull.vAccumulated_Delta_AngMomentum
inc .CollisionHull.dNumSimultaneousCollisions
.endif
MethodEnd
1. We calculate the Impulse using either with or without Friction.
2. The impulse we calculated is in Contact coords, so we convert it to World coords.
3. We split the impulse into Linear and Angular components, and apply them to the Body(s).
As you can see, I'm not applying them directly - I'm accumulating them so that each Body has the average effect of N impulses applied to it simultaneously - this isnt strictly correct, the real problem is a simultaneous equation, and the real solution involves 'Jacobians', something I'm not too familiar with... but the average of the change in momenta due to N impulses is pretty close and looks believable at a fraction of the cost of the Jacobian solution.
Although theres a lot going on here, the really interesting stuff is buried out of sight.. Now I suppose you want to see the new methods whose names were exposed in that source?
We'll do that in the next post :)
PS : Attached is an update of the Handy Math includefile, which contains some new stuff including the __NegScaleFloat3 macro, as well as 90% of the other odd-looking macros you've seen me using (the remainder can be found in OA32's fMath includefile)
ONLY REGISTERED MEMBERS CAN SEE DOWNLOAD LINKS.
Here's the first new method we have to look at.
This one calculates the velocity of a Point on a moving Body, and expresses it in Contact coordinates , where the X axis is the Collision Normal, and the Y and Z axes are 'PLANAR'.
;Calculate the Velocity at CollisionPoint (in Contact coordinates)
;Returns Vec3 on fpu
Method CollisionHull.calculateLocalVelocity,uses esi,pTargetConfig,pmatContactToWorld, pvrelativeContactPosition
LOCAL velocity:Vec3
LOCAL accVelocity:Vec3
LOCAL vout:Vec3
SetObject esi
; Work out the velocity of the contact point.
mov eax,pvrelativeContactPosition
mov edx,pTargetConfig
CrossProduct .configuration.AngularVelocity ,.Vec3
__AddToFloat3 velocity, .configuration.CMVelocity
__StowFloat3 velocity
; Turn the velocity into contact-coordinates.
mov eax,pmatContactToWorld
Vec3_transMult .Mat33, velocity
__StowFloat3 vout
; Convert "the change in velocity that was due to acceleration In This Frame" into contact-coordinates.
mov eax,pmatContactToWorld
Vec3_transMult .Mat33, .configuration.CMVelocityChange
__StowFloat3 accVelocity
; We ignore any component of acceleration in the contact normal direction,
; we are only interested in 'planar' acceleration
mov accVelocity.x,0
; Add the planar velocities - if there's enough friction they will be removed during velocity resolution
__AddFloat3 vout, accVelocity
MethodEnd
Nobody noticed the little bug in there so I won't mention EBX.
Here's your daily dose of fiber :)
Please excuse the 'debug' macros, as this code was recently debugged and implemented.
The goal of this method is to produce a vector that contains an Impulse, given in ContactSpace coordinates. Remember that in Contact space, the X axis points along our Collision Normal, so X will contain the impulse along the collision normal, the one we really want. The Y and Z axes, which lay along the collision surface, are totally not used in a Frictionless collision, so we just set them to Zero.
You can take a look back to the Response method if you're interested in what happens to the returned impulse vector.
One method to go, and I believe we're done here.
Next we'll be looking at the FRICTIONAL impulse calculation.
Take the time to study the above method, as the Frictional version is quite similar, although a little more complex (we'll be using those Y and Z fields of the contactspace impulse).
After we've covered the last method, you'll realize that THIS method could have been implemented without any mention of 'contact space' at all.
The reason we've wasted our time converting to and from contact space is to give you the foundation knowledge to understand the Frictional version, where this WILL be required.
Here's your daily dose of fiber :)
Please excuse the 'debug' macros, as this code was recently debugged and implemented.
;desiredDeltaVelocity is passed silently as input on FPU
Method CollisionHull.CalculateFrictionlessImpulse,uses esi,pvimpulseContactOut,pvContactNormal, pmatContactBasis,pvRelativeContactPositionA, pvRelativeContactPositionB, pconfigA,pBodyB,pconfigB
LOCAL deltaVelWorld:Vec3
LOCAL deltaVelocity:real8
LOCAL desiredDeltaVelocity:real8
fstp desiredDeltaVelocity ;<--
DbgFloat desiredDeltaVelocity
SetObject esi
; Build a vector that shows the change in velocity (in world space)
; for a UNIT impulse in the direction of the contact normal.
;Calculate the Torque per Unit of Impulse
mov eax,pvContactNormal
mov ecx,pvRelativeContactPositionA
CrossProduct .Vec3, .Vec3
__StowFloat3 deltaVelWorld ;torquePerUnitImpulse = CrossProduct(relativeContactPosition,contactNormal)
lea edx,deltaVelWorld
DbgVec3 edx,"torquePerUnitImpulse"
;Use InverseInertiaTensor to convert Torque into rotationPerUnitImpulse
mov ecx,pconfigA
Vec3_mult_Mat33 .configuration.InverseWorldInertiaTensor, deltaVelWorld
__StowFloat3 deltaVelWorld
lea edx,deltaVelWorld
DbgVec3 edx,"rotationPerUnitImpulse"
;Find the Linear Velocity that is due to rotation and that is along the collision normal
mov ecx,pvRelativeContactPositionA ;velocityPerUnitImpulse = CrossProduct(rotationPerUnitImpulse, relativeContactPosition)
CrossProduct deltaVelWorld, .Vec3
__StowFloat3 deltaVelWorld
; Work out the change in velocity in contact coordinates.
;ImpulseDenominator = Body.OneOverMass + DotProduct(velocityPerUnitImpulse,CollisionNormal)
mov eax,pvContactNormal
DotProduct deltaVelWorld, .Vec3
fadd .OneOverMass
fstp deltaVelocity
; Check if we need the second body's data
.if pBodyB!=0
int 3
; Go through the same transformation sequence again
mov eax,pvContactNormal
mov ecx,pvRelativeContactPositionB
CrossProduct .Vec3, .Vec3
__StowFloat3 deltaVelWorld
mov ecx,pconfigB
Vec3_mult_Mat33 .configuration.InverseWorldInertiaTensor, deltaVelWorld
__StowFloat3 deltaVelWorld
mov ecx,pvRelativeContactPositionB
CrossProduct deltaVelWorld, .Vec3
__StowFloat3 deltaVelWorld
; Add the change in velocity due to rotation
mov eax,pvContactNormal
DotProduct deltaVelWorld, .Vec3
fadd deltaVelocity
mov ecx,pBodyB
fadd .CollisionHull.OneOverMass
fstp deltaVelocity
.endif
; Calculate the required size of the impulse
mov edx,pvimpulseContactOut
fld desiredDeltaVelocity
fdiv deltaVelocity
fstp .Vec3.x
mov .Vec3.y,0
mov .Vec3.z,0
DbgVec3 edx,"IMPULSE contact space"
MethodEnd
The goal of this method is to produce a vector that contains an Impulse, given in ContactSpace coordinates. Remember that in Contact space, the X axis points along our Collision Normal, so X will contain the impulse along the collision normal, the one we really want. The Y and Z axes, which lay along the collision surface, are totally not used in a Frictionless collision, so we just set them to Zero.
You can take a look back to the Response method if you're interested in what happens to the returned impulse vector.
One method to go, and I believe we're done here.
Next we'll be looking at the FRICTIONAL impulse calculation.
Take the time to study the above method, as the Frictional version is quite similar, although a little more complex (we'll be using those Y and Z fields of the contactspace impulse).
After we've covered the last method, you'll realize that THIS method could have been implemented without any mention of 'contact space' at all.
The reason we've wasted our time converting to and from contact space is to give you the foundation knowledge to understand the Frictional version, where this WILL be required.
Here's the final piece of the puzzle.
With this method in place, we have a fully-functional collision detection and response system that accurately simulates collisions between multiple rigid bodies comprised of materials with known physical properties.
With this method in place, we have a fully-functional collision detection and response system that accurately simulates collisions between multiple rigid bodies comprised of materials with known physical properties.
;desiredDeltaVelocity is passed silently as input on FPU
Method CollisionHull.CalculateFrictionImpulse,uses esi,pvimpulseContactOut, pmatContactBasis,pvContactVelocity,pvrelativeContactPositionA,pvrelativeContactPositionB,pconfigA,pBodyB,pconfigB, LP_frictions
LOCAL impulseToTorque:Mat33, tempmat:Mat33
LOCAL deltaVelWorld:Mat33, deltaVelWorld2:Mat33
LOCAL deltaVelocity:Mat33, impulseMatrix:Mat33
LOCAL velKill:Vec3
LOCAL inverseMass:real8, planarImpulse:real8
LOCAL desiredDeltaVelocity:real8
fstp desiredDeltaVelocity
SetObject esi
fld .OneOverMass
fstp inverseMass
; The equivalent of a cross product in matrices is multiplication
; by a skew symmetric matrix - we build the matrix for converting
; between linear and angular quantities.
mov eax,pvrelativeContactPositionA
Mat33_star impulseToTorque, .Vec3
DbgMat33 impulseToTorque, "impulseToTorque matrix"
; Build the matrix to convert contact impulse --> change in velocity in world coordinates.
mov eax,pconfigA
invoke Mat33_multiply,addr tempmat, addr impulseToTorque, addr .configuration.InverseWorldInertiaTensor
invoke Mat33_multiply,addr deltaVelWorld, addr tempmat , addr impulseToTorque
Mat33_neg deltaVelWorld
DbgMat33 deltaVelWorld,"impulseToDeltaVelocity WORLDSPACE"
.if pBodyB!=0
mov eax,pvrelativeContactPositionA
Mat33_star impulseToTorque, .Vec3
invoke Mat33_multiply,addr tempmat, addr impulseToTorque, addr .configuration.InverseWorldInertiaTensor
invoke Mat33_multiply,addr deltaVelWorld2, addr tempmat , addr impulseToTorque
Mat33_neg deltaVelWorld2
; Add to the total delta velocity.
;deltaVelWorld += deltaVelWorld2;
lea eax,deltaVelWorld
lea edx,deltaVelWorld2
Mat33_addto eax,edx
fld inverseMass
mov eax,pBodyB
fadd .CollisionHull.OneOverMass
fstp inverseMass
.endif
; Do a change-of-basis to convert into contact coordinates.
mov eax,pmatContactBasis
Mat33_Transpose deltaVelocity, .Mat33 ;Contact-To-World transform
DbgMat33 deltaVelocity,"Transpose of ContactToWorld = WorldToContact"
invoke Mat33_multiply,addr tempmat, addr deltaVelocity,addr deltaVelWorld
invoke Mat33_multiply,addr deltaVelocity, addr tempmat, pmatContactBasis
DbgMat33 deltaVelocity,"impulseToDeltaVelocity CONTACTSPACE"
; Add in the linear velocity change
fld inverseMass
fadd deltaVelocity.m00
fstp deltaVelocity.m00
fld inverseMass
fadd deltaVelocity.m11
fstp deltaVelocity.m11
fld inverseMass
fadd deltaVelocity.m22
fstp deltaVelocity.m22
DbgMat33 deltaVelocity,"impulseMatrix"
; Invert to get the impulse needed per unit velocity
invoke Mat33_inv,addr impulseMatrix ,addr deltaVelocity
DbgMat33 impulseMatrix,"impulseMatrix inverted"
; Find the target velocities to kill
mov edx,pvContactVelocity
fld desiredDeltaVelocity
fstp velKill.x
fld .Vec3.y
fchs
fstp velKill.y
fld .Vec3.z
fchs
fstp velKill.z
; Find the impulse to kill target velocities
Vec3_mult_Mat33 impulseMatrix, velKill
mov eax,pvimpulseContactOut
__StowFloat3 .Vec3
; Check if theres enough PLANAR IMPULSE to overcome STATIC FRICTION
fld .Vec3.y
fmul st(0),st(0)
fld .Vec3.z
fmul st(0),st(0)
fadd
fsqrt
fstp planarImpulse
;if (planarImpulse > impulseContact.x * STATIC friction)
fld .Vec3.x
mov edx,LP_frictions
fmul .MyTableElement._Static
fsub planarImpulse
fstpReg eax
.ifBitSet eax,BIT31
; We need to use DYNAMIC friction
mov eax,pvimpulseContactOut
fld .Vec3.y
fdiv planarImpulse
fstp .Vec3.y
fld .Vec3.z
fdiv planarImpulse
fstp .Vec3.z
;impulseContact.x = deltaVelocity.data[0] +
; deltaVelocity.data[1]*friction*impulseContact.y +
; deltaVelocity.data[2]*friction*impulseContact.z;
;impulseContact.x = desiredDeltaVelocity / impulseContact.x;
;impulseContact.y *= friction * impulseContact.x;
;impulseContact.z *= friction * impulseContact.x;
fld desiredDeltaVelocity
fld deltaVelocity.m11
fmul .MyTableElement._Dynamic
fmul .Vec3.y
fadd deltaVelocity.m00
fld deltaVelocity.m22
fmul .MyTableElement._Dynamic
fmul .Vec3.z
fadd
fdiv
fstp .Vec3.x
fld .Vec3.y
fmul .MyTableElement._Dynamic
fstp .Vec3.y
fld .Vec3.z
fmul .MyTableElement._Dynamic
fstp .Vec3.z
.endif
MethodEnd
Well, perhaps not the final piece.
Having implemented all this stuff in a demo, I found that my demo was 'mostly stable' - but that under certain conditions, bodies were able to penetrate the world bounding planes and cause the simulator to enter into an infinite loop.
The above system was supposed to make penetrations an impossible thing, but nonetheless under rare circumstances the system is failing, and the immediate problem is unexpected (inter)penetration of one or more bodies.
So, we're going to need to resolve these penetrations when they occur.
One way we could do that is simply to 'move' the bodies out of penetration along the collision normal, but that is not accurate.. it assumes that only LINEAR motion was responsible for the penetration, and indeed it causes error in our body's state since we didn't correct the velocity(s) and other stuff.. so what are we gonna do?
To resolve a penetration, we wish to 'go backwards in Time' until the Penetration becomes a Collision, resolve the collision, and then 'go forwards in Time' so that our Body is back in sync with the rest of the simulation. But our goal won't be to calculate how much negative Time we need...
The reason that we didn't detect this Penetration BEFORE it occurred (as we planned to) is that there's a relatively high velocity at the point of collision, so the Time involved is so small that it becomes problematic in terms of numerical precision... it escaped our temporal search , it must be really small, so we can't use Time.
We'll need to directly manipulate the physical states of our body(s), and work with the Velocities instead.
Let us assume that our system can at least tell us how deep the penetration is..
The penetration was caused by the motion of the body (or bodies).
We must recognize that this motion has two components - linear and angular.
Each component is partly responsible for the collision.
Can we determine HOW MUCH each is responsible? Yep, we sure can.
If we review the calculation of a collision response impulse, the first thing we did was calculate the total velocity at the collisionpoint due to both angular and linear motion.
We were able to determine how much velocity was due to Angular motion alone, yes?.
That is what we need now.
We need to find two Scaling values which represent the contributions of the linear and angular motions to the total penetration depth, and from them, calculate the translation and rotation required to bring that penetration depth to Zero.
I think we'll tackle this one in stages, its not huge, but its mathematically hairy - even though it only uses stuff we've already seen.
Having implemented all this stuff in a demo, I found that my demo was 'mostly stable' - but that under certain conditions, bodies were able to penetrate the world bounding planes and cause the simulator to enter into an infinite loop.
The above system was supposed to make penetrations an impossible thing, but nonetheless under rare circumstances the system is failing, and the immediate problem is unexpected (inter)penetration of one or more bodies.
So, we're going to need to resolve these penetrations when they occur.
One way we could do that is simply to 'move' the bodies out of penetration along the collision normal, but that is not accurate.. it assumes that only LINEAR motion was responsible for the penetration, and indeed it causes error in our body's state since we didn't correct the velocity(s) and other stuff.. so what are we gonna do?
To resolve a penetration, we wish to 'go backwards in Time' until the Penetration becomes a Collision, resolve the collision, and then 'go forwards in Time' so that our Body is back in sync with the rest of the simulation. But our goal won't be to calculate how much negative Time we need...
The reason that we didn't detect this Penetration BEFORE it occurred (as we planned to) is that there's a relatively high velocity at the point of collision, so the Time involved is so small that it becomes problematic in terms of numerical precision... it escaped our temporal search , it must be really small, so we can't use Time.
We'll need to directly manipulate the physical states of our body(s), and work with the Velocities instead.
Let us assume that our system can at least tell us how deep the penetration is..
The penetration was caused by the motion of the body (or bodies).
We must recognize that this motion has two components - linear and angular.
Each component is partly responsible for the collision.
Can we determine HOW MUCH each is responsible? Yep, we sure can.
If we review the calculation of a collision response impulse, the first thing we did was calculate the total velocity at the collisionpoint due to both angular and linear motion.
We were able to determine how much velocity was due to Angular motion alone, yes?.
That is what we need now.
We need to find two Scaling values which represent the contributions of the linear and angular motions to the total penetration depth, and from them, calculate the translation and rotation required to bring that penetration depth to Zero.
I think we'll tackle this one in stages, its not huge, but its mathematically hairy - even though it only uses stuff we've already seen.
We need to separate the angular and linear parts of this problem... we're trying to find the amount of the penetration due to EACH component, based on their 'relative intensities'.
Here's the most important method we'll require to solve this problem - it tells us the momentum of a given Body, along a given direction :)
If we wanted the Velocity, we could remove the Mass component (momemtum=velocity * mass).
;**Returns float on FPU
;Given a Collision Normal and a Contact Point,
;this method calculates the Linear Momentum
;that is due ONLY to rotation, and which is in
;the direction of the Collision Normal.
Method CollisionHull.Calculate_AngularMomentum_along_Normal,uses esi,pvContactNormal, pvrelativeContactPosition, pconfig
LOCAL angularInertiaWorld:Vec3
SetObject esi
;Calculate Torque per unit impulse at contact point
mov edx,pvContactNormal
mov eax,pvrelativeContactPosition
CrossProduct .Vec3, .Vec3 ;torque = R cross N = change in angular momentum over time
__StowFloat3 angularInertiaWorld
mov edx,pconfig
;Convert Torque into Angular Momentum
Vec3_mult_Mat33 .configuration.InverseWorldInertiaTensor, angularInertiaWorld ;rotation = !I * torque = change in angular velocity
__StowFloat3 angularInertiaWorld
;Find the Linear part of that Angular motion
mov edx,pvrelativeContactPosition
CrossProduct angularInertiaWorld , .Vec3
__StowFloat3 angularInertiaWorld
;Find the Linear part that is along the Normal
mov edx,pvContactNormal
DotProduct angularInertiaWorld , .Vec3
MethodEnd
This method returns a floating point value representing the given Body's momentum in the given direction... since its a direction, you MUST provide a Normal vector. We'll be passing the Collision Normal, but we COULD use this method to find the Body's momentum in ANY given direction.
It's telling us how much the Rotation Motion is responsible for total linear motion at a given Point on the Body.
Our solver will use this value to determine how much the Linear Motion is responsible for total linear motion at that Point, so we then know how much EACH is responsible.
From there we can solve our problem.
The entrypoint for our penetration-solver is called Resolve_Penetration (oddly enough).
Here's the first few lines from that method.
You can see we're calculating the linear momentum along the collision normal which is solely due to the Rotation of each Body, as well as the Total linear momentum (again, along the collision normal). Don't get confused about the terms Inertia and Momentum ... the only real difference is that one is the signed opposite of the other... although not strictly correct, I use these terms interchangeably in my variable names.
Now let's see how we use those results to calculate that part of PenetrationDepth that is attributable to the Linear and to the Angular motions:
Now we are in business 8)
From these two partial-PenDepth values we can calculate the changes in linear and angular position that are required to balance the equations such that the PenDepth 'disappears' (is equal to zero), and then adjust the linear and angular Velocities to suit.
We'll see how to do that next time :)
Here's the first few lines from that method.
;Corrects a Penetration of two Bodies, but does not resolve the Collision
Method CollisionHull.Resolve_Penetration, uses esi, pvContactNormal, pvrelativeContactPosition, pOtherBody, pSourceA,pTargetA,pSourceB,pTargetB,fPenetrationDepth:real8
LOCAL angularInertiaWorld:Vec3
LOCAL angularMoveA:real4, angularMoveB:real4
LOCAL linearMoveA:real4, linearMoveB:real4
LOCAL linearChangeA:Vec3, linearChangeB:Vec3
LOCAL angularChangeA:Vec3, angularChangeB:Vec3
LOCAL totalMoveA:real4,totalMoveB:real4
local maxMagnitude:real8
local totalInertia:real8
local angularMomentumA:real8, angularMomentumB:real8
LOCAL targetAngularDirectionA:Vec3,targetAngularDirectionB:Vec3
LOCAL projection:Vec3
LOCAL ftemp
SetObject esi
; We need to find the momentum of each object in the direction
; of the contact normal which is due to rotation only...
OCall esi.Calculate_AngularMomentum_along_Normal, pvContactNormal, pvrelativeContactPosition, pSourceA
fstp angularMomentumA
OCall pOtherBody::CollisionHull.Calculate_AngularMomentum_along_Normal, pvContactNormal, pvrelativeContactPosition, pSourceB
fst angularMomentumB
;Now calculate the TOTAL momentum with respect to the collision normal
fadd angularMomentumA
fadd .OneOverMass ;Add the Linear component of momentum
mov edx,pOtherBody
fadd .CollisionHull.OneOverMass
fstp totalInertia
You can see we're calculating the linear momentum along the collision normal which is solely due to the Rotation of each Body, as well as the Total linear momentum (again, along the collision normal). Don't get confused about the terms Inertia and Momentum ... the only real difference is that one is the signed opposite of the other... although not strictly correct, I use these terms interchangeably in my variable names.
Now let's see how we use those results to calculate that part of PenetrationDepth that is attributable to the Linear and to the Angular motions:
;Determine how much the Linear and Angular motions each contributed to the Penetration,
;and thereby, how much each must contribute to the Correction.
fld angularMomentumA
fdiv totalInertia
fmul fPenetrationDepth
fstp angularMoveA
;
fld .OneOverMass
fdiv totalInertia
fmul fPenetrationDepth
fstp linearMoveA
;
fld angularMomentumB
fdiv totalInertia
fmul fPenetrationDepth
fchs
fstp angularMoveB
;
mov edx,pOtherBody
fld .CollisionHull.OneOverMass
fdiv totalInertia
fmul fPenetrationDepth
fchs
fstp linearMoveB
Now we are in business 8)
From these two partial-PenDepth values we can calculate the changes in linear and angular position that are required to balance the equations such that the PenDepth 'disappears' (is equal to zero), and then adjust the linear and angular Velocities to suit.
We'll see how to do that next time :)
I've added a little 'floor grid' to my demo so that we can better visualize the collisions of the demo body and the floor plane.
Here's a wishlist of other stuff I basically forgot about.
Chances are this list will grow!
-Improved support for Edge and Face collisions (reduced number of Point collision responses)
-Some basic gui menu controls for manipulating and resetting the demo simulation
-()
And now we return you to your feature program.
Here's a wishlist of other stuff I basically forgot about.
Chances are this list will grow!
-Improved support for Edge and Face collisions (reduced number of Point collision responses)
-Some basic gui menu controls for manipulating and resetting the demo simulation
-()
And now we return you to your feature program.
OK lets get some closure on the current issue.
Here's the entire procedure, with small recent changes.
Take note of the 'angularLimit' threshold, which is used to limit the amount of Rotational correction, which places more load on the Linear correction.
This threshold is mostly for body/body collisions, where the bodies may vary greatly in mass, causing acute projections for the body with smaller mass.
And you'll also be interested in this:
Please note that I am NOT correcting the Linear Velocity - the last few lines of code are incomplete.
Also, note that the "angularLimit" threshold is completely arbitrary - you pretty much have to experiment to find a nice value but I find that 0.2 works nicely for me.
When you've had a chance to look that code over, I'll post an updated version of the simulator's CheckCollisions method, which calls this new penetration-correction stuff.
Here's the entire procedure, with small recent changes.
Take note of the 'angularLimit' threshold, which is used to limit the amount of Rotational correction, which places more load on the Linear correction.
This threshold is mostly for body/body collisions, where the bodies may vary greatly in mass, causing acute projections for the body with smaller mass.
;Corrects a Penetration but does not resolve the Collision
Method CollisionHull.Resolve_Penetration, uses esi, pvContactNormal, pvrelativeContactPositionA, pvrelativeContactPositionB,pOtherBody, pSourceA,pTargetA,pSourceB,pTargetB,fPenetrationDepth:real8
LOCAL angularInertiaWorld:Vec3
LOCAL angularMoveA:real4, angularMoveB:real4
LOCAL linearMoveA:real4, linearMoveB:real4
LOCAL linearChangeA:Vec3, linearChangeB:Vec3
LOCAL angularChangeA:Vec3, angularChangeB:Vec3
LOCAL totalMoveA:real4,totalMoveB:real4
local maxMagnitude:real8
local totalInertia:real8
local angularMomentumA:real8, angularMomentumB:real8
LOCAL targetAngularDirectionA:Vec3,targetAngularDirectionB:Vec3
LOCAL projection:Vec3
LOCAL ftemp
SetObject esi
DbgFloat fPenetrationDepth
; We need to find the momentum of each object in the direction
; of the contact normal which is due to rotation only...
OCall esi.Calculate_AngularMomentum_along_Normal, pvContactNormal, pvrelativeContactPositionA, pSourceA
fabs
DbgFST angularMomentumA
.if pOtherBody!=0
OCall pOtherBody::CollisionHull.Calculate_AngularMomentum_along_Normal, pvContactNormal, pvrelativeContactPositionB, pSourceB
fabs
DbgFST angularMomentumB
fadd
mov edx,pOtherBody
fadd .CollisionHull.OneOverMass
.endif
;Now calculate the TOTAL momentum with respect to the collision normal
fadd .OneOverMass ;Add the Linear component of momentum
DbgFSTP totalInertia
;Determine how much the Linear and Angular motions each contributed to the Penetration,
;and thereby, how much each must contribute to the Correction.
fld angularMomentumA
fdiv totalInertia
fmul fPenetrationDepth
DbgFSTP angularMoveA
;whatevers left over is the linear correction
fld fPenetrationDepth
fsub angularMoveA
DbgFSTP linearMoveA
;
; To avoid angular projections that are too great
; (when mass is large but inertia tensor is small)
; we will limit the angular move.
mov edx,pvContactNormal
mov eax,pvrelativeContactPositionA
DotProduct .Vec3,.Vec3
DbgFSTP ftemp
__ScaleFloat3 .Vec3, ftemp
__AddToFloat3 projection, .Vec3
; Use the small angle approximation for the sine of the angle
; i.e. the magnitude would be sine(angularLimit) * projection.magnitude
; but we approximate sine(angularLimit) to angularLimit
; maxMagnitude = angularLimit * projection.magnitude
DotProduct projection,projection
fsqrt
fmul angularLimit
fst maxMagnitude
fchs
DbgFSTP ftemp
fMin angularMoveA, ftemp ;if (angularMoveA < -maxMagnitude)
fstpReg eax
.if eax==angularMoveA
DbgWarning "FLOOR CLAMP on angular correction"
fld linearMoveA
fadd angularMoveA
DbgFSTP totalMoveA
fld maxMagnitude
fchs
DbgFSTP angularMoveA
fld totalMoveA
fsub angularMoveA
DbgFSTP linearMoveA
.else
fMax angularMoveA, maxMagnitude ;elseif (angularMoveA > maxMagnitude)
fstpReg eax
.if eax==angularMoveA
DbgWarning "CEILING CLAMP on angular correction"
fld linearMoveA
fadd angularMoveA
DbgFSTP totalMoveA
fld maxMagnitude
DbgFSTP angularMoveA
fld totalMoveA
fsub angularMoveA
DbgFSTP linearMoveA
.endif
.endif
; We have the linear amount of movement required by ROTATING the rigid body (in angularMoveA).
; We now need to calculate the desired rotation to achieve that.
.if (angularMoveA == 0 || angularMoveA==80000000h)
; Easy case - no angular movement means no rotation.
fldz
fst angularChangeA.x
fst angularChangeA.y
fstp angularChangeA.z
.else
; Work out the direction we'd like to rotate in.
; targetAngularDirection = CrossProduct (relativeContactPositionA,contactNormal)
mov edx,pvrelativeContactPositionA
mov eax,pvContactNormal
CrossProduct .Vec3, .Vec3 ;torque
__StowFloat3 targetAngularDirectionA ;Its a Direction because we used a Normal vector
; Convert the 'Torque' into 'change in Angular Velocity'
mov eax,pTargetA
Vec3_mult_Mat33 .configuration.InverseWorldInertiaTensor, targetAngularDirectionA ;= desired angular velocity
;Scale the Angular Velocity by our DESIRED angular velocity
fld angularMoveA
fdiv angularMomentumA
fmul st(3),st(0)
fmul st(2),st(0)
fmul
__StowFloat3 angularChangeA
lea edx,angularChangeA
DbgVec3 edx,"angularChangeA"
;The result is the change in angular momentum required to produce the desired change in angular velocity
.endif
; Linear change is easier - it is just the linear movement along the contact normal.
mov edx,pvContactNormal
__ScaleFloat3 .Vec3, linearMoveA
__StowFloat3 linearChangeA
lea edx,linearChangeA
DbgVec3 edx,"linearChangeA"
; Now we can apply the values we've calculated.
OCall esi.Apply_Correction, addr linearChangeA, addr angularChangeA, pSourceA, pTargetA
;------------------------------------------------------------------------------------------------------
;Now we go through almost the exact same sequence, for the Other Body
.if pOtherBody!=0
fld angularMomentumB
fdiv totalInertia
fmul fPenetrationDepth
fchs
DbgFSTP angularMoveB
mov edx,pOtherBody
fld .CollisionHull.OneOverMass
fdiv totalInertia
fmul fPenetrationDepth
fchs
DbgFSTP linearMoveB
mov edx,pvContactNormal
mov eax,pvrelativeContactPositionB
DotProduct .Vec3,.Vec3
DbgFSTP ftemp
__ScaleFloat3 .Vec3, ftemp
__AddToFloat3 projection, .Vec3
; Use the small angle approximation for the sine of the angle
; i.e. the magnitude would be sine(angularLimit) * projection.magnitude
; but we approximate sine(angularLimit) to angularLimit
; maxMagnitude = angularLimit * projection.magnitude
DotProduct projection,projection
fsqrt
fmul angularLimit
fst maxMagnitude
fchs
DbgFSTP ftemp
fMin angularMoveB, ftemp
fstpReg eax
.if eax==angularMoveB
fld linearMoveB
fadd angularMoveB
DbgFSTP totalMoveB
fld maxMagnitude
fchs
DbgFSTP angularMoveB
fld totalMoveB
fsub angularMoveB
DbgFSTP linearMoveB
.else
fMax angularMoveB, maxMagnitude
fstpReg eax
.if eax==angularMoveB
fld linearMoveB
fadd angularMoveB
DbgFSTP totalMoveB
fld maxMagnitude
DbgFSTP angularMoveB
fld totalMoveB
fsub angularMoveB
DbgFSTP linearMoveB
.endif
.endif
.if (angularMoveB == 0 || angularMoveB==80000000h)
fldz
fst angularChangeB.x
fst angularChangeB.y
fstp angularChangeB.z
.else
mov edx,pvContactNormal
mov eax,pvrelativeContactPositionB
CrossProduct .Vec3, .Vec3
__StowFloat3 targetAngularDirectionB
mov eax,pTargetB
Vec3_mult_Mat33 .configuration.InverseWorldInertiaTensor, targetAngularDirectionB
fld angularMoveB
fdiv angularMomentumB
fmul st(3),st(0)
fmul st(2),st(0)
fmul
__StowFloat3 angularChangeB
lea edx,angularChangeB
DbgVec3 edx,"angularChangeB"
.endif
mov edx,pvContactNormal
__ScaleFloat3 .Vec3, linearMoveB
__StowFloat3 linearChangeB
OCall pOtherBody::CollisionHull.Apply_Correction, addr linearChangeB, addr angularChangeB, pSourceB, pTargetB
.endif
;Now we COULD move Forwards in time if we were really worried about time-syncing our bodies...
;...OR we can simply live with the tiny time-glitch
MethodEnd
And you'll also be interested in this:
;Called by Resolve_Penetration
Method CollisionHull.Apply_Correction,uses esi, pvLinearChange, pvAngularChange, pSource, pTarget
LOCAL matTemp:Mat33
LOCAL matTemp2:Mat33
SetObject esi
; Apply the change in position
mov edx,pTarget
mov eax,pvLinearChange
__AddFloat3 .configuration.CMPosition,.Vec3
__StowFloat3 .configuration.CMPosition
; And the change in orientation
mov edx,pTarget
mov eax,pvAngularChange
__AddFloat3 .configuration.AngularMomentum,.Vec3
__StowFloat3 .configuration.AngularMomentum
;We've changed the POSITION and ANGULAR MOMENTUM
;We need to fix up the Linear and Angular Velocities
;AngVelocity from AngMomentum as seen in INTEGRATE...
;Make a new Orientation matrix based on the Angular Change
mov eax,pvAngularChange
Mat33_star matTemp, .Vec3
mov edx,pSource
invoke Mat33_multiply,addr matTemp2, addr .configuration.Orientation, addr matTemp
mov eax,pSource
lea edx,.configuration.Orientation ;Source.Orientation
mov ebx,pTarget
lea ebx,.configuration.Orientation ;Target.Orientation
lea ecx,matTemp2 ;Offset.Orientation
Mat33_Add ebx, ecx, edx
mov edx,pTarget
invoke OrthonormalizeOrientation,addr .configuration.Orientation
;Fix up the Inertia Tensor for the new Orientation
mov edx,pTarget
Mat33_SimilarityTransform .configuration.InverseWorldInertiaTensor, matTemp, .InverseBodyInertiaTensor, .configuration.Orientation
; Extract new Angular Velocity from the new Tensor and Momentum
; Target.AngularVelocity = Target.InverseWorldInertiaTensor * Target.AngularMomentum
mov eax,edx
Vec3_mult_Mat33 .configuration.InverseWorldInertiaTensor, .configuration.AngularMomentum
__StowFloat3 .configuration.AngularVelocity
mov edx,pSource
;Ensure that our worldspace vertex array is up to date
OCall esi.TransformVertices,addr .configuration.CMPosition, addr .configuration.Orientation
;Scale the LINEAR Velocity as follows:
;CMVelocity and CMVelocityChange from CHANGE IN POSITION
;S = (new target.CMPosition - old target.CMPosition) / (old target.CMPosition-src.CMPosition)
;target.CMVelocity = target.CMVelocity * S
;target.CMVelocityChange = target.CMVelocityChange * S
MethodEnd
Please note that I am NOT correcting the Linear Velocity - the last few lines of code are incomplete.
Also, note that the "angularLimit" threshold is completely arbitrary - you pretty much have to experiment to find a nice value but I find that 0.2 works nicely for me.
When you've had a chance to look that code over, I'll post an updated version of the simulator's CheckCollisions method, which calls this new penetration-correction stuff.
Theres a reason why I did not bother to adjust the linear velocity.
And this will lead to a separate discussion of the solver, where we will see that positional correction is NOT NEEDED.
But I digress.
For now, please note that when Penetration occurs, it is ALWAYS SHALLOW - its limited to the TIME EPSILON of the "FindExactCollisionTime" method... so when we advanced the system into the penetration state, we did so by the smallest possible margin.
The positional error, for which we corrected, was small.
The velocity correction is small too.
So small.
Important?
Conservation of energy.
We corrected the position, and not the velocity.
What if we DIDNT correct the position - and used the penetration point for collision resolution?
The velocity issue would disappear - and we'd have a little angular error instead..
Would you not say that is more acceptable than wearing, or having to correct, the error in TOTAL momentum?
And this will lead to a separate discussion of the solver, where we will see that positional correction is NOT NEEDED.
But I digress.
For now, please note that when Penetration occurs, it is ALWAYS SHALLOW - its limited to the TIME EPSILON of the "FindExactCollisionTime" method... so when we advanced the system into the penetration state, we did so by the smallest possible margin.
The positional error, for which we corrected, was small.
The velocity correction is small too.
So small.
Important?
Conservation of energy.
We corrected the position, and not the velocity.
What if we DIDNT correct the position - and used the penetration point for collision resolution?
The velocity issue would disappear - and we'd have a little angular error instead..
Would you not say that is more acceptable than wearing, or having to correct, the error in TOTAL momentum?