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

``ifndef RigidBodyIDRigidBodyID 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.dataVec3 struct	X REAL4 ?	Y REAL4 ?	Z REAL4 ?Vec3 endsVec3_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 .ZendmQuaternion struct	X REAL4 ?	Y REAL4 ?	Z REAL4 ?	W REAL4 ?Quaternion endsMat33 structM0 REAL4 ?M1 REAL4 ?M2 REAL4 ?M3 REAL4 ?M4 REAL4 ?M5 REAL4 ?M6 REAL4 ?M7 REAL4 ?M8 REAL4 ?Mat33 endsMat33_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 eaxendm; ??????????????????????????????????????????????????????????????????????????????????????????????????;DEFINE SOME GLOBALS ("CLASS STATICS"); ??????????????????????????????????????????????????????????????????????????????????????????????????r4_0_0f	 REAL4 0.0fr4_half	 REAL4 0.50fr4_3_0f  REAL4 3.0fr4_5_0f	 REAL4 5.0fr4_12_0f REAL4 12.0f; gravity, y accel = -gRigidBody_g		REAL4 0.0f;precalculated Step sizes (for speedup)RigidBody_RKh	REAL4 0.0fRigidBody_RKh2	REAL4 0.0fRigidBody_RKh4	REAL4 0.0fRigidBody_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, protObjectEnd; ??????????????????????????????????????????????????????????????????????????????????????????????????; ??????????????????????????????????????????????????????????????????????????????????????????????????;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 .IzzMethodEnd; ??????????????????????????????????????????????????????????????????????????????????????????????????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,0MethodEnd; ??????????????????????????????????????????????????????????????????????????????????????????????????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,0MethodEnd; ??????????????????????????????????????????????????????????????????????????????????????????????????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 .IzxMethodEnd; ??????????????????????????????????????????????????????????????????????????????????????????????????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	retVec3Transform 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..
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