TRigidBody

• allStop
• angularMomentum
• angularMomentum:
• centerMass
• centerMassWorld
• frame
• velocity
• velocity:

• collideFloor:
• collidePlane:offset:

• computeForce:delta:
• globalTransformUpdate
• impulse:at:
• integrate:
• orthoNormalize:

• initializeWithFrame:mass:

• isRigidBody

	self velocity: B3DVector3 new.
	self angularMomentum: B3DVector3 new.

	^ angularMomentum.

	angularMomentum _ am.

	^ centerMass.

	^ centerMassWorld.

	^ frame.

	^ velocity.

	velocity _ v.

 
	^ frame collideFloor: floor

	^ frame collidePlane: normal offset: offset.

	force _ force + (f*mass).

"	force _ force - (dampingLinear * mass * velocity).
	torque _ torque - (dampingAngular * angularMomentum)."

	

	" This is actually called when the globalTransform needs to be updated because the frame has changed. This means that we only recalculate centerMassWorld as necessary."
	centerMassWorld _ globalTransform localPointToGlobal: centerMass.

	| rc |
	rc _ r-self centerMassWorld.

	velocity _ velocity + (impulse * oneOverMass).
	angularMomentum _ angularMomentum + (rc cross: impulse).

	| position orientation av angularVelocity |

"	energy = 0.0 ifTrue:[^ self]."
	
	position _ self translation.
	orientation _ self orientation.

	velocity _ force * oneOverMass * deltaTime + velocity.
	position _ velocity * deltaTime + position.

	angularMomentum _ torque * deltaTime + angularMomentum.

	inverseWorldInertiaTensor _ (orientation orthoNormInverse composedWithLocal:
		inverseInertiaTensor) composedWithLocal: orientation.

	angularVelocity _  inverseWorldInertiaTensor localPointToGlobal: angularMomentum.
	av _ deltaTime * angularVelocity.

	orientation _ orientation + (orientation composedWithLocal: (B3DMatrix4x4 skew: av)).
	orientation _ self orthoNormalize: orientation.

	self orientation: orientation.
	self translation: position.
	torque _ B3DVector3 new.
	force _ B3DVector3 new.

	| rmat r1 r2 r3 |

	rmat _ B3DMatrix4x4 new.
	r1 _ mat row1.
	r2 _ mat row2.
	r3 _ mat row3.

	r1 _ r1 normalize.
	r3 _ (r1 cross: r2)normalize.
	r2 _ (r3 cross: r1)normalize.
	
	rmat a11: r1 x.
	rmat a12: r1 y.
	rmat a13: r1 z.
	rmat a21: r2 x.
	rmat a22: r2 y.
	rmat a23: r2 z.
	rmat a31: r3 x.
	rmat a32: r3 y.
	rmat a33: r3 z.
	rmat a44: 1.0.
	^ rmat.
	

	| it |
	super initialize.
" This object can only have one frame associated with it as the rigid body object for now. #addChild: should not be used."
	self singleParent: true.
	frame _ frm.
	frm objectOwner: nil. "This forces the pointer to look at the parent for objectOwner."
	self addChild: frame.
	mass _ ms.
	it _ frame inertiaTensor.
	it ifNotNil:[
		it mass: ms.
		inertiaTensor _ it result.
" The inertiaTensor is not a homogeneous matrix (I don't think), so I can't just flip axes here. Doesn't matter anyway, as this is only done in initialize."
		inverseInertiaTensor _ inertiaTensor inverseTransformation.
		centerMass _ it centerMass.
		oneOverMass _ 1.0/mass.].
	
	velocity _ B3DVector3 new.
	momentum _ B3DVector3 new.
	force _ B3DVector3 new.
	torque _ B3DVector3 new.
	angularMomentum _ B3DVector3 new.
	dampingLinear _ 0.5.
	dampingAngular _2.0.
	energy _ 0.
	self visible: false.
	^self

	^ true.