Jedive ODE Wrapper

Here is some code for testing the quaternions. The quaternion to euler routine is in the UpdateEntities() function. Not correct right now:

Include "ode.bb"

Type Entity
	Field body
	Field geom
	Field ent
End Type

;Create 3D stuff
Graphics3D 800,600,32,2

l = CreateLight()
RotateEntity l, 15, 30, 0
cam = CreateCamera()
PositionEntity cam, 0, 100, -100
RotateEntity cam, 45, 0, 0
;TurnEntity cam,0,45,0,1
CameraClsColor cam, 180, 180, 180

;Make floor texture
ftex = CreateTexture(32,32,9)
etex = CreateTexture(32,32,9)
SetBuffer TextureBuffer(ftex)
ClsColor 255, 255, 255 : Cls
Color 255, 0, 0
Rect 0, 0, 16, 16
Rect 16, 16, 16, 16
SetBuffer TextureBuffer(etex)
ClsColor 255, 255, 255 : Cls
Color 0, 0, 255
Rect 0, 0, 16, 16
Rect 16, 16, 16, 16
SetBuffer BackBuffer()
ScaleTexture ftex, 32, 32

;Setup ODE world
Global world=dWorldCreate()
Global space=dHashSpaceCreate(0)
Global contactgroup=dJointGroupCreate(0)
dWorldSetGravity world, 0, -150*ODE_SCALE, 0
dWorldSetAutoDisableFlag world, True
dContactSetMaxContacts 4
dContactSetMode dContactSoftERP+dContactSoftCFM
dContactSetMu 0.98
dWorldSetContactSurfaceLayer world,0.001

;Create collision plane
dCreatePlane(space, 0, 1, 0, 0)
p=CreatePlane()
PositionEntity p, 0, 0, 0
EntityTexture p, ftex
EntityAlpha p, 0.5
m=CreateMirror()
PositionEntity m,0,0,0

Const ODE_SCALE#=0.01

;Loop
While Not KeyHit(1)
	If MilliSecs()-lasttime>1000
		lasttime=MilliSecs()
		ent.Entity=CreateEntity(world, space, Rand(-10, 10), 100, Rand(-10, 10), Rand(10, 30), Rand(10, 30), 15)
		EntityTexture ent\ent, etex
		EndIf
	dSpaceCollide space, world, contactgroup
	dWorldQuickStep world, 0.02
	dJointGroupEmpty contactgroup
	UpdateEntities
	RenderWorld
	Flip
Wend

;Free ODE stuff
dJointGroupDestroy(contactgroup)
dSpaceDestroy(space)
dWorldDestroy(world)
dCloseODE

End

Function CreateEntity.Entity(world_, space_, x#, y#, z#, w#, h#, d#)
	ent.Entity = New Entity
	ent\body = dBodyCreate(world_)
	ent\geom = dCreateBox(space_, w#*ODE_SCALE, h#*ODE_SCALE, d#*ODE_SCALE)
	ent\ent = CreateCube()
	ScaleMesh ent\ent, w#/2, h#/2, d#/2
	EntityShininess ent\ent, 0.7

	mass = dMassCreate()
	dMassSetBox mass, 1,w#*ODE_SCALE, h#*ODE_SCALE, d#*ODE_SCALE
	dBodySetMass ent\body, mass
	dMassDestroy mass

	dBodySetPosition ent\body, x#*ODE_SCALE, y#*ODE_SCALE, z#*ODE_SCALE
	dGeomSetPosition ent\geom,x,y,z
	dGeomSetBody ent\geom, ent\body

	Return ent
End Function

Function UpdateEntities()
	For ent.Entity = Each Entity
		dGeomGetPosition ent\geom
		PositionEntity ent\ent, dVectorX#()/ODE_SCALE, dVectorY#()/ODE_SCALE, dVectorZ#()/ODE_SCALE
		
		dBodyGetQuaternion ent\body
		
		x#=dVectorX()
		y#=dVectorY()
		z#=dVectorZ()
		w#=dVectorW()
		sp#=-2.0*(y*z+w*x)
		If Abs(sp)>0.9999
			roll#=1.570796*sp
			pitch#=-ATan2(-x*z-w*y,0.5-y*y-z*z)
			yaw#=0.0
			Else
			roll#=ASin(sp)
			pitch#=-ATan2(x*z-w*y,0.5-x*x-y*y)
			yaw#=-ATan2(x*y-w*z,0.5-x*x-z*z)
			EndIf

		RotateEntity ent\ent,pitch,yaw,roll

		If dBodyIsEnabled(ent\body) Then EntityColor ent\ent, 255, 255, 255 Else EntityColor ent\ent, 255, 255, 0
	Next
End Function

Here is the code of interest:

		dBodyGetQuaternion ent\body		
		x#=dVectorX()
		y#=dVectorY()
		z#=dVectorZ()
		w#=dVectorW()
		sp#=-2.0*(y*z+w*x)
		If Abs(sp)>0.9999
			pitch#=1.570796*sp
			roll#=-ATan2(-x*z-w*y,0.5-y*y-z*z)
			yaw#=0.0
			Else
			pitch#=ASin(sp)
			roll#=-ATan2(x*z-w*y,0.5-x*x-y*y)
			yaw#=-ATan2(x*y-w*z,0.5-x*x-z*z)
			EndIf
		RotateEntity ent\ent,pitch,yaw,roll