Just a teaser - I'm messing with it..


ifndef RigidBodyID
RigidBodyID equ 00d0d0h

;=========================================================================
;Title   : RigidBody
;Description   : Class for a 3D rigid body with 4th order Runge-Kutta integrator.
;Original Author  : Richard Chaney
;Original Version : 4.0 - June 2000
;Current  Author  : Evil Homer
;Current  Version : 1.0 - July 2005
;=========================================================================

;=========================================================================
;Notes:
; To set up a rigid body do as follows :
; 1.  Call 'setGravity' to set RigidBody_g (gravity is always along the y axis)
; 2.  Create a rigid primitive with 'setBox', 'setCylinder' or 'setSphere'.
; 3.  Join primitives together if needed with 'combine'.
; 4.  Call 'diagonalise' to make moment of inertia matrix diagonal. It also initialise various bits.
; 5.  Don't forget to transform your graphical object and collision detection object by the transform :
; newCoord = diagRotPos * oldCoord + diagLinPos
; 6.  Call 'initialise' with initial parameters
;
;  Bodies can be integrated with Euler or 4th order Runge-Kutta.
; Runge-Kutta is recommended as it is much more accurate for a given step size,
; however the step takes 4 times as long to calculate.
;
; The following Steps must Always be performed, regardless
; of which Integration method we choose to use...
;
; 1.  Call 'setStepSize' to set size of step (applies to all rigid bodies)
; 2.  Call 'RKInit'
; 3.  Use linPos, linVel, rotPos and rotVel to determine forces on body.
; Add forces and torques with provided functions, or add them to linForce and rotForce directly.
; 4.  Calculate constraint forces if necessary and add to linForce and rotForce
;
; To update it with Euler method do :
; 5.  Call 'eulerStep'
; 6.  Draw body at linPos, linVel, rotPos and rotVel.
;
; To update it with Runge-Kutta method do :
; 5.  Call 'RKStep1'
; 6.  Repeat step 2 and 3.
; 7.  Call 'RKStep2'
; 8.  Repeat step 2 and 3.
; 9.  Call 'RKStep3'
; 10. Repeat step 2 and 3.
; 11. Call 'RKStep4'
; 12. Draw body at linPos, linVel, rotPos and rotVel.
;
; To go back to the previous position after a step is made call 'undo'.
; This is useful when a collision is detected.
;=========================================================================

;include Matrix.inc


.data
Vec3 struct
X REAL4 ?
Y REAL4 ?
Z REAL4 ?
Vec3 ends
Vec3_mac macro pOut, pA, pB, fS
;Out = A + B * scalar
fld pB.X
fmul fS
fadd pA.X
fstp pOut.X
fld pB.Y
fmul fS
fadd pA.Y
fstp pOut.Y
fld pB.Z
fmul fS
fadd pA.Z
fstp pOut.Z
endm


__LoadFloat3 macro blah
fld .X
fld .Y
fld .Z
endm

Quaternion struct
X REAL4 ?
Y REAL4 ?
Z REAL4 ?
W REAL4 ?
Quaternion ends

Mat33 struct
M0 REAL4 ?
M1 REAL4 ?
M2 REAL4 ?
M3 REAL4 ?
M4 REAL4 ?
M5 REAL4 ?
M6 REAL4 ?
M7 REAL4 ?
M8 REAL4 ?
Mat33 ends

Mat33_mac macro pTarget,pSource1,pSource2,fS
;target = A + B * scalar
push eax
push ebx
push ecx
push esi

lea eax,pTarget
mov ebx,pSource1
mov esi,pSource2
xor ecx,ecx
.while ecx<9
push ecx
shl ecx,2
fld  REAL4 ptr
fmul fs
fadd REAL4 ptr
fstp REAL4 ptr
pop ecx
inc ecx
.endw


pop esi
pop ecx
pop ebx
pop eax
endm
; ??????????????????????????????????????????????????????????????????????????????????????????????????
;DEFINE SOME GLOBALS ("CLASS STATICS")
; ??????????????????????????????????????????????????????????????????????????????????????????????????
r4_0_0f REAL4 0.0f
r4_half REAL4 0.50f
r4_3_0f  REAL4 3.0f
r4_5_0f REAL4 5.0f
r4_12_0f REAL4 12.0f
; gravity, y accel = -g
RigidBody_g REAL4 0.0f

;precalculated Step sizes (for speedup)
RigidBody_RKh REAL4 0.0f
RigidBody_RKh2 REAL4 0.0f
RigidBody_RKh4 REAL4 0.0f
RigidBody_RKh6 REAL4 0.0f

.code
; ??????????????????????????????????????????????????????????????????????????????????????????????????
; DEFINE THE RIGIDBODY CLASS OBJECT
; ??????????????????????????????????????????????????????????????????????????????????????????????????
Object RigidBody,RigidBodyID,Primer

; Runge-Kutta integration stuff
    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,{}

; properties of body
    DefineVariable mass,REAL4
    DefineVariable oneOverMass,REAL4
;-mass * g
    DefineVariable minusMG,REAL4
;centre of mass (in old body coords)
    DefineVariable cOfM,Vec3,{}

; Ix = sum of mx
    DefineVariable Ix,REAL4
    DefineVariable Iy,REAL4
    DefineVariable Iz,REAL4
;Ixx = sum of m(y^2 + z^2)
    DefineVariable Ixx,REAL4
    DefineVariable Iyy,REAL4
    DefineVariable Izz,REAL4
; Ixy = sum of mxy
    DefineVariable Ixy,REAL4
    DefineVariable Iyz,REAL4
    DefineVariable Izx,REAL4
                                  ;these are in old body coords

; transform from old body coords to diagonalised body coords.
    DefineVariable diagRotPos,Mat33,{}
    DefineVariable diagLinPos,Vec3,{};  Rd = diagRotPos * Rb + diagLinPos
    DefineVariable diagIxx,REAL4
    DefineVariable oneOverDiagIxx,REAL4;transformed moment of inertia (diagonalised body coords)
    DefineVariable diagIyy,REAL4
    DefineVariable oneOverDiagIyy,REAL4;also 1 / Ixx * h for Euler integrater
    DefineVariable diagIzz,REAL4
    DefineVariable oneOverDiagIzz,REAL4;
    DefineVariable diagIyyMinusIzzOverIxx,REAL4;
    DefineVariable diagIzzMinusIxxOverIyy,REAL4;
    DefineVariable diagIxxMinusIyyOverIzz,REAL4;

; (Iyy - Izz) / Ixx * h, etc..                                           
    DefineVariable diagIzzMinusIyy,REAL4;
    DefineVariable diagIxxMinusIzz,REAL4;
    DefineVariable diagIyyMinusIxx,REAL4;
; linear damping (-ve and multiplied by mass)
    DefineVariable linKD,REAL4
;Angular damping (-ve and multiplied by Ixx, etc..)
    DefineVariable rotKDx, REAL4
    DefineVariable rotKDy, REAL4   
    DefineVariable rotKDz,REAL4               


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

;rotational position.. transform from diagonalised body coord to world coords is
;Rw = rotPos * Rd + linPos
DefineVariable rotPos,Mat33,{}
;quaternion of above matrix (matrix and quaternion are kept the same)                
    DefineVariable qRotPos,Quaternion,{};
;rotational velocity (diagonalised body coords)   
DefineVariable rotVel,Vec3,{}

; current forces on body
DefineVariable linForce,Vec3,{}
DefineVariable rotForce,Vec3,{}

; functions to set up rigid body mass distribution
StaticMethod setBox,REAL4,REAL4,REAL4,REAL4;(float xRadius, float yRadius, float zRadius, float mass);
    StaticMethod setCylinder,REAL4,REAL4,REAL4;(float radius, float height, float mass);
    StaticMethod setSphere,REAL4,REAL4 ;(float radius, float mass);
    StaticMethod clear
;    StaticMethod combine,Pointer,Pointer,Pointer;pother:ptr RigidBody, protPos:ptr Mat33, plinPos:ptr Vec3

; functions for initialising rigid body
    StaticMethod diagonalise
    RedefineMethod Init,Pointer,Pointer,Pointer,REAL4,REAL4; poLinPos, poLinVel, poQRotPos, poRotVel,flinKD, frotKD
StaticMethod setGravity,REAL4
StaticMethod setStepSize,REAL4

; functions to update from one frame to another
    StaticMethod RKInit
    StaticMethod eulerStep
    StaticMethod RKStep1
    StaticMethod RKStep2
    StaticMethod RKStep3
    StaticMethod RKStep4
    StaticMethod undo

; functions to add force and torque
StaticMethod addBodyTorque,Pointer
StaticMethod addWorldTorque,Pointer
StaticMethod addWorldWorldForce,Pointer,Pointer ;pforce,ppos
StaticMethod addWorldBodyForce,Pointer,Pointer ;pforce,ppos
StaticMethod addBodyBodyForce,Pointer,Pointer ;pforce,ppos

; functions to get current position and velocity of points
StaticMethod findWorldPos,Pointer,Pointer;pbodyPos, pworldPos
StaticMethod findBodyPos,Pointer,Pointer;pWorldPos, pbodyPos
StaticMethod findBodyWorldVel,Pointer,Pointer;pbodyPos, pworldVel
StaticMethod findWorldWorldVel,Pointer,Pointer;pworldPos, pworldVel
    StaticMethod findWorldPosVel,Pointer,Pointer,Pointer;pbodyPos, pworldPos, pworldVel

; functions for energy and momentum (for testing)
    StaticMethod energy
    StaticMethod momentum,Pointer,Pointer ;plin, prot
ObjectEnd
; ??????????????????????????????????????????????????????????????????????????????????????????????????
; ??????????????????????????????????????????????????????????????????????????????????????????????????
;DEFINE THE RIGIDBODY CLASS METHODS
; ??????????????????????????????????????????????????????????????????????????????????????????????????

Method RigidBody.setBox,uses esi, fxRadius:REAL4, fyRadius:REAL4, fzRadius:REAL4, fmass:REAL4
;=====================================================================
;Set the body as a rectangular box centred on the origin.
;xRadius, yRadius, zRadius = half the size of the box
;fmass = total mass
;=====================================================================
SetObject esi
m2m .mass , fmass
mov .cOfM.X,0
mov .cOfM.Y,0
mov .cOfM.Z,0
mov .Ix,0
mov .Iy,0
mov .Iz,0
mov .Ixy,0
mov .Iyz,0
mov .Izx,0

; Ixx = mass / 3.0 * (yRadius * yRadius + zRadius * zRadius);
fld fmass
fld fyRadius
fmul st(0),st(0)
fld fzRadius
fmul st(0),st(0)
fadd
fmul r4_3_0f
fdiv
fstp .Ixx

; Iyy = mass / 3.0 * (zRadius * zRadius + xRadius * xRadius);
fld fmass
fld fzRadius
fmul st(0),st(0)
fld fxRadius
fmul st(0),st(0)
fadd
fmul r4_3_0f
fdiv
fstp .Iyy

; Izz = mass / 3.0 * (xRadius * xRadius + yRadius * yRadius);
fld fmass
fld fxRadius
fmul st(0),st(0)
fld fyRadius
fmul st(0),st(0)
fadd
fmul r4_3_0f
fdiv
fstp .Izz
MethodEnd

; ??????????????????????????????????????????????????????????????????????????????????????????????????

Method RigidBody.setCylinder,uses esi,fradius:REAL4, fheight:REAL4, fmass:REAL4
;================================================================
;Set the body as a cylinder centred on the origin.
;Cylinder goes along X axis from -height/2 to +height/2.
;fradius, fheight = size of cylinder
;fmass = total mass
;================================================================
SetObject esi
m2m .mass,fmass
mov .cOfM.X,0
mov .cOfM.Y,0
mov .cOfM.Z,0
    mov .Ix,0
    mov .Iy,0
    mov .Iz,0 
   
; Ixx = mass * radius * radius / 2.0;
fld fradius
fmul st(0),st(0)
fmul fmass
fmul r4_half
fstp .Ixx

; Iyy = Izz = mass / 12.0 * (3 * radius * radius + height * height);
fld fmass
fld fradius
fmul st(0),st(0)
fmul r4_3_0f
fld fheight
fmul st(0),st(0)
fadd
fmul r4_12_0f
fdiv
fst .Iyy
fstp .Izz

;    Ixy = Iyz = Izx = 0.0;
mov .Ixy,0
mov .Iyz,0
mov .Izx,0
MethodEnd

; ??????????????????????????????????????????????????????????????????????????????????????????????????

Method RigidBody.setSphere,uses esi,fradius:REAL4, fmass:REAL4
;================================================================
;Set the body as a sphere centred on the origin.
;fradius = size of sphere
;fmass = total mass
;================================================================
SetObject esi
m2m .mass,fmass
mov .cOfM.X,0
mov .cOfM.Y,0
mov .cOfM.Z,0
    mov .Ix,0
    mov .Iy,0
    mov .Iz,0 

; Ixx = Iyy = Izz = mass * radius * radius * 2.0 / 5.0;
    fld fradius
    fmul st(0),st(0)
    fmul fmass
    fadd st(0),st(0)
    fdiv r4_5_0f
    fst .Ixx
    fst .Iyy
    fstp .Izz

;    Ixy = Iyz = Izx = 0.0;
mov .Ixy,0
mov .Iyz,0
mov .Izx,0
MethodEnd

; ??????????????????????????????????????????????????????????????????????????????????????????????????

Method RigidBody.clear,uses esi
;==============================================
;Set the body as zero mass.
;May be useful if 'combine' is being used.
;==============================================
SetObject esi
fld r4_0_0f
fst .mass
fst .cOfM.X
fst .cOfM.Y
fst .cOfM.Z
fst .Ix
fst .Iy
fst .Iz
fst .Ixx
fst .Iyy
fst .Izz
fst .Ixy
fst .Iyz
fstp .Izx
MethodEnd

; ??????????????????????????????????????????????????????????????????????????????????????????????????


Vec3Transform proc pDest,pr, protPos, plinPos
;Transform a point to a new coords system.
; pDest:pVec3 - output vec3
;      pr:pVec3                    -  point in coord system 1
;      protPos:pMat33              -  rotation from system 1 to 2
;      plinPos:pVec3                -  translation from system 1 to 2
;

mov eax,pr
mov ebx,protPos
mov ecx,plinPos
mov edi,pDest

; x = r.x * rotPos.m[0] + r.y * rotPos.m[1] + r.z * rotPos.m[2] + linPos.x;
fld .Vec3.X
fmul .Mat33.M0
fld .Vec3.Y
fmul .Mat33.M1
fadd
fld .Vec3.Z
fmul .Mat33.M2
fadd
fadd .Vec3.X
fstp .Vec3.X

; y = r.x * rotPos.m[3] + r.y * rotPos.m[4] + r.z * rotPos.m[5] + linPos.y;
fld .Vec3.X
fmul .Mat33.M3
fld .Vec3.Y
fmul .Mat33.M4
fadd
fld .Vec3.Z
fmul .Mat33.M5
fadd
fadd .Vec3.Y
fstp .Vec3.Y

; z = r.x * rotPos.m[6] + r.y * rotPos.m[7] + r.z * rotPos.m[8] + linPos.z;
fld .Vec3.X
fmul .Mat33.M6
fld .Vec3.Y
fmul .Mat33.M7
fadd
fld .Vec3.Z
fmul .Mat33.M8
fadd
fadd .Vec3.Z
fstp .Vec3.Z
ret
Vec3Transform endp

; ??????????????????????????????????????????????????????????????????????????????????????????????????





endif


Posted on 2005-08-15 03:43:08 by Homer
Decided to implement Math functions in a separate testbed (see MathTools thread), I'll come back to this.
Posted on 2005-08-16 16:10:11 by Homer
More code was implemented, see attachment if interested..
Attachments:
Posted on 2005-08-19 11:56:06 by Homer
That's pretty much all the "hard stuff" out of the way, except for the integrator methods, which are so very similar that I will likely try to come up with a universal method for these.. (I dislike redundancy in my code..)

After I complete this code module, I'll be looking for people to help me write code to apply "constraints" to the physics objects, such as various kinds of Joints and other goodies expected of a physics engine, because the aim is to get to "ragdoll" stage as quickly as possible, and I have no experience with this stuff or the math behind it.

Have a nice day :)
Posted on 2005-08-19 12:34:17 by Homer
Good stuff.

One question though, why are ya living in the 1990's with all the FPU math ? :D
Go SSE on those rx^2 + rz^2 things, you'll just end up having to rewrite it anyways when your testing and the slow FPU is bottlenecking everything.
Posted on 2005-08-22 20:19:32 by r22
Hi, I wanted to ask what the Quaternions xyzw represents when converted from a 3x3 matrix and how and why to convert?
Posted on 2005-08-28 16:49:50 by inFinie
The quaternions represent an orientation, ie, a 3D rotation about an arbitrary axis.
3x3 Matrices are used to represent rotational inertia ("angular velocity"), from which we can derive a 3D orientation at any moment in time.. that is to say, we are also using 3x3 matrices to represent rotation in 3-space.
So why bother with quaternions?

Matrices are subject to compound error from numerical drift, and quaternions are less prone to compounded error.. also, it's easier to interpolate between two quaternions than it is to interpolate between two matrices (think about that).

We can extract a quaternion from a 3x3 rotation matrix by observing the plane equation, thus our quaternion is "unit length".

I have deliberately omitted speaking about diagonalising the moment of inertia matrix via Eigen Vectors, but I think I covered everything else :)

Posted on 2005-08-29 00:27:55 by Homer