JV-ODE Physics Wrapper

Blitz3D Forums/Blitz3D Userlibs/JV-ODE Physics Wrapper

The forum is locked. These are the original blitzbasic.com forums, kept so they can still be read. No new topics, replies or edits can be made here. To join in, head over to the Blitz3D+ forums.

Wayne
 #1
The theme is make it stop. I made a number of changes to the car trimesh demo including UpdateGeoms(). Now everthing that moves will turn green and when stopped will turn red. This new demo shows friction damping, and use of autodisable with thresholds.

enjoy

; ###################################################################################################
; #									  JV-ODE TriMesh Car Demo										#
; #									Code by Jim Williams (VIP3R)									#		#
; #						Feel free to use this code, any credit appreciated							#
; ###################################################################################################


AppTitle "JV-ODE TriMesh Car Demo"

Include "ode.bb"

Graphics3D 800,600,0,2

Global CMass#=200			; ### Car Mass
Global WMass#=8				; ### Wheel Mass

Global WorldERP#=1			; ### World Error Correction
Global WorldFriction#=64	; ### World Friction
Global HingeTorque#=24		; ### Hinge Torque
Global SuspensionHS#=0.01	; ### Hinge Suspension Hardness/Softness (Higher=Softer - Lower=Harder)

Global Speed#=0
Global Steer#=0

Global Car
Global CGeom
Global CMesh

Global CarStartY=40

Dim Wheel(4)
Dim WGeom(4)
Dim Joint(4)

Type ODEGeom
	Field body
	Field geom
	Field mesh
End Type

; ###################################################################################################

; ### Setup ODE

Global World=dWorldCreate()
Global Space=dHashSpaceCreate(0)
Global ContactGroup=dJointGroupCreate(0)

dWorldSetGravity(World,0,-1,0)
dWorldSetERP(World,WorldERP)

dContactSetMode(dContactSlip1)
dContactSetMu(WorldFriction)

dWorldSetAutoDisableFlag (World, 1)
; ### Create car body

ode.ODEGeom=New ODEGeom
ode\body=dBodyCreate(World)
Car=ode\body
dBodySetRotation(ode\body,0,0,0)
dBodySetPosition(ode\body,0,CarStartY,0)
mass=dMassCreate()
dMassSetBox(mass,1,3,1,4)
dMassSetBoxTotal(mass,CMass,3,1,4)
dMassTranslate(mass,0,-1,0)
dBodySetMass(ode\body,mass)
dMassDestroy(mass)
ode\geom=dCreateBox(Space,3,1,4)
CGeom=ode\geom
dGeomSetBody(ode\geom,ode\body)
ode\mesh=CreateCube()
CMesh=ode\mesh
ScaleMesh ode\mesh,1.5,0.5,2
RotateMesh ode\mesh,0,0,0
PositionMesh ode\mesh,0,0,0
EntityColor ode\mesh,40,80,255
EntityAlpha ode\mesh,1

;Tell car body to disable at new threshold
	dBodySetAutoDisableFlag(ode\body,1)
	dBodySetAutoDisableAngularThreshold(ode\body,.05)
	dBodySetAutoDisableLinearThreshold(ode\body,.05)
	dBodySetAutoDisableSteps (ode\body, 1)

; ### Create wheels

For count=1 To 4
	ode.ODEGeom=New ODEGeom
	ode\body=dBodyCreate(World)
	Wheel(count)=ode\body
	dBodySetRotation(ode\body,0,0,0)
	dBodySetPosition(ode\body,0,0,0)
	mass=dMassCreate()
	dMassSetSphere(mass,1,0.7)
	dMassSetSphereTotal(mass,WMass,0.7)
	dBodySetMass(ode\body,mass)
	dMassDestroy(mass)
	ode\geom=dCreateSphere(Space,0.7)
	WGeom(count)=ode\geom
	dGeomSetBody(ode\geom,ode\body)
	ode\mesh=CreateCylinder(8)
	ScaleMesh ode\mesh,0.7,0.25,0.7
	RotateMesh ode\mesh,0,0,90
	PositionMesh ode\mesh,0,0,0
	EntityColor ode\mesh,255,255,255
	
	dBodySetAutoDisableFlag(ode\body,1)
	dBodySetAutoDisableAngularThreshold(ode\body,.05)
	dBodySetAutoDisableLinearThreshold(ode\body,.05)
	dBodySetAutoDisableSteps (ode\body, 1)
Next

dBodySetPosition(Wheel(1),-2,CarStartY-0.5,+2)
dBodySetPosition(Wheel(2),+2,CarStartY-0.5,+2)
dBodySetPosition(Wheel(3),-2,CarStartY-0.5,-2)
dBodySetPosition(Wheel(4),+2,CarStartY-0.5,-2)

; ### Create some objects

SeedRnd MilliSecs()

For spheres=1 To 20
	ode.ODEGeom=New ODEGeom
	ode\body=dBodyCreate(World)
	dBodySetRotation(ode\body,0,0,0)
	dBodySetPosition(ode\body,Rnd(-100,100),44,Rnd(-100,100))
	ode\geom=dCreateSphere(Space,4)
	dGeomSetBody(ode\geom,ode\body)
	ode\mesh=CreateSphere()
	
	dBodySetAutoDisableFlag(ode\body,1)
	dBodySetAutoDisableAngularThreshold(ode\body,.05)
	dBodySetAutoDisableLinearThreshold(ode\body,.05)
	dBodySetAutoDisableSteps (ode\body, 1)
	
	ScaleMesh ode\mesh,4,4,4
	RotateMesh ode\mesh,0,0,0
	PositionMesh ode\mesh,0,0,0
	EntityColor ode\mesh,Rnd(255),Rnd(255),Rnd(255)
	EntityAlpha ode\mesh,1
	EntityShininess ode\mesh,0.7
Next

; ### Create light

Global Light=CreateLight()

RotateEntity Light,45,-90,0
LightColor Light,255,255,255
AmbientLight 130,130,130

; ### Create camera

Global CameraPivot=CreatePivot(CMesh)
PositionEntity CameraPivot,0,2,-7

Global Camera=CreateCamera()
CameraClsColor Camera,0,0,0
CameraRange Camera,1,1000

; ### Create plane

dCreatePlane(Space,0,1,0,0)

Plane=CreatePlane()

EntityAlpha Plane,0.8

PlaneTexture=CreateTexture(128,128,9)

ClsColor 0,200,80
Cls

Color 255,255,255

Rect 0,0,64,64,1
Rect 64,64,64,64,1

CopyRect 0,0,128,128,0,0,BackBuffer(),TextureBuffer(PlaneTexture)
ScaleTexture PlaneTexture,20,20
EntityTexture Plane,PlaneTexture,0,0

Mirror=CreateMirror()

; ###################################################################################################

; ### CREATE TRIMESH

demomesh=LoadMesh("media\terrain.b3d")
ScaleMesh demomesh,1,0.7,1
trimesh=CreateTriMesh(demomesh)
dGeomSetRotation(trimesh,0,0,0)
dGeomSetPosition(trimesh,0,0.05,0)
RotateMesh demomesh,0,0,0
PositionMesh demomesh,0,0.05,0

EntityColor demomesh,255,255,255
EntityAlpha demomesh,1

demotexture=LoadTexture("media\terrain.png")
ScaleTexture demotexture,0.08,0.08
EntityTexture demomesh,demotexture

; ###################################################################################################

SetupCar()

While Not KeyHit(1)

	UpdateKeys()

	PTime=MilliSecs()

	UpdateCar()

	UpdateGeoms()

	dSpaceCollide(Space,World,ContactGroup)
	UpdateFriction
	
	dWorldQuickStep(World,0.1)
	
	dJointGroupEmpty(ContactGroup)

	PhysicsTime#=MilliSecs()-PTime

	UpdateCam()

	UpdateWorld

	RenderWorld

	Text 0,0,"JV-ODE Version "+dGetVersion()
	Text 0,15,"Physics Time:"+PhysicsTime
	Text 0,30,"Car Speed:"+Speed
	Text 640,0,"A - Accelerate"
	Text 640,15,"Z - Brake/Reverse"
	Text 640,30,"Arrows - Steering"
	Text 640,45,"Space - Reset Car"

	Flip

Wend

dJointGroupDestroy(ContactGroup)
dSpaceDestroy(Space)
dWorldDestroy(World)
dCloseODE()

End

; ###################################################################################################

Function SetupCar()

For count=1 To 4
	Joint(count)=dJointCreateHinge2(World,0)
	dJointAttach(Joint(count),Car,Wheel(count))
	dJointSetHinge2Anchor(Joint(count),dBodyGetPositionX(Wheel(count)),dBodyGetPositionY(Wheel(count)),dBodyGetPositionZ(Wheel(count)))
	dJointSetHinge2Axis1(Joint(count),0,1,0)
	dJointSetHinge2Axis2(Joint(count),-1,0,0)
	dJointSetHinge2Param(Joint(count),dParamSuspensionERP,0.8)
	dJointSetHinge2Param(Joint(count),dParamSuspensionCFM,SuspensionHS)
	If count>2
		dJointSetHinge2Param(Joint(count),dParamLoStop,0)
		dJointSetHinge2Param(Joint(count),dParamHiStop,0)
		End If
Next

Speed=0

End Function

; ###################################################################################################

Function UpdateKeys()

If KeyDown(30)=1
	Speed=Speed+0.08
	If Speed>30 Then Speed=30
	End If

If KeyDown(44)=1
	Speed=Speed-0.1
	If Speed<-10 Then Speed=-10
	If Speed>0 Then Speed=Speed*0.97
	End If

If KeyDown(30)=0 And KeyDown(44)=0 Then Speed=Speed*0.99

If KeyDown(203)=1 Or KeyDown(205)=1
	If KeyDown(203)=1
		Steer=0.5
		Else
		Steer=-0.5
		End If
	Else
	Steer=0
	End If

If KeyHit(57)=1
	Speed=0
	dBodySetRotation(Car,0,0,0)
	End If

If speed>.01 
	For count=1 To 4
		dBodyEnable(Wheel(count))
	Next

	dBodyEnable(Car)
Else
	speed=0
End If

End Function

; ###################################################################################################

Function UpdateCar()

;For count=1 To 4
;	dBodyEnable(Wheel(count))
;Next

;dBodyEnable(Car)

For count=1 To 4
	dJointSetHinge2Param(Joint(count),dParamVel2,Speed)
	dJointSetHinge2Param(Joint(count),dParamFMax2,HingeTorque)
Next

For count=1 To 2
	angle#=Steer-dJointGetHinge2Angle1(Joint(count))
	dJointSetHinge2Param(Joint(count),dParamVel,angle)
	dJointSetHinge2Param(Joint(count),dParamFMax,400)
Next

End Function

; ###################################################################################################

Function UpdateCam()

PositionEntity CameraPivot,0,3,-12

camx#=EntityX(CameraPivot,1)-EntityX(Camera)
camy#=EntityY(CameraPivot,1)-EntityY(Camera)
camz#=EntityZ(CameraPivot,1)-EntityZ(Camera)

TranslateEntity Camera,camx*0.5,camy*0.5,camz*0.5

PointEntity Camera,CMesh

End Function

; ###################################################################################################

Function UpdateGeoms()

For ode.ODEGeom=Each ODEGeom
	If dBodyIsEnabled(ode\body)
		RotateEntity ode\mesh,dGeomGetPitch#(ode\geom),dGeomGetYaw#(ode\geom),dGeomGetRoll#(ode\geom)
		PositionEntity ode\mesh,dGeomGetPositionX#(ode\geom),dGeomGetPositionY#(ode\geom),dGeomGetPositionZ#(ode\geom)
	End If
Next

End Function

; ###################################################################################################

Function CreateTriMesh(mesh)

tricount=0
vertcount=0

For count=1 To CountSurfaces(mesh)
	tricount=tricount+CountTriangles(GetSurface(mesh,count))
	vertcount=vertcount+CountVertices(GetSurface(mesh,count))
Next

tribank=CreateBank(tricount*3*4)
vertbank=CreateBank(vertcount*4*4)

offset=0
trioffset=0
vertoffset=0

For count=1 To CountSurfaces(mesh)
	surface=GetSurface(mesh,count)
	For vert=0 To CountVertices(surface)-1
		PokeFloat(vertbank,offset,VertexX(surface,vert))
		offset=offset+4
		PokeFloat(vertbank,offset,VertexY(surface,vert))
		offset=offset+4
		PokeFloat(vertbank,offset,VertexZ(surface,vert))
		offset=offset+8
	Next
	For tri=0 To CountTriangles(surface)-1
		For corner=0 To 2
			trivert=TriangleVertex(surface,tri,corner)+vertoffset
			PokeInt(tribank,trioffset,trivert)
			trioffset=trioffset+4
		Next
	Next
	vertoffset=vertoffset+CountVertices(surface)
Next

trimeshdata=dGeomTriMeshDataCreate()

dGeomTriMeshDataBuildSimple(trimeshdata,vertbank,vertcount,tribank,tricount*3)

Return dCreateTriMesh(Space,trimeshdata)

End Function

; ###################################################################################################

Function AngularDamp(body,damp#)
  dBodyAddTorque body,dBodyGetAngularVelX(body)*-damp,dBodyGetAngularVelY(body)*-damp,dBodyGetAngularVelZ(body)*-damp
End Function

; ###################################################################################################

Function LinearDamp(body,damp#)
  dBodyAddForce body,dBodyGetLinearVelX(body)*-damp,dBodyGetLinearVelY(body)*-damp,dBodyGetLinearVelZ(body)*-damp
End Function

; ###################################################################################################

Function UpdateFriction()

For ode.ODEGeom=Each ODEGeom
	; color code if body enabled
	If dBodyIsEnabled(ode\body)=0
		EntityColor ode\mesh,255,0,0
	Else
		EntityColor ode\mesh,0,255,0
	End If
	
	; add friction force if body enabled  
	If dBodyIsEnabled(ode\body)
		numjoints=dBodyGetNumJoints(ode\body)
		If numjoints>0 
		DebugLog "joints=" + Str(joints)
			For joints=0 To numjoints-1
				joint1=dBodyGetJoint(ode\body,joints)
				If dJointGetType(joint1)=dJointTypeContact
					
					; Get linear velocity vector
			  		ax#=dBodyGetLinearVelX#(ode\body)
			  		ay#=dBodyGetLinearVelY#(ode\body)
				    az#=dBodyGetLinearVelZ#(ode\body)
				
					; Get Angular velocity vector
			  		bx#=dBodyGetAngularVelX#(ode\body)
			  		by#=dBodyGetAngularVelY#(ode\body)
					bz#=dBodyGetAngularVelZ#(ode\body)
					
			  		; Calculate magnitudes
					ForceA#=Sqr(ax*ax+ay*ay+az*az)
					ForceB#=Sqr(bx*bx+by*by+bz*bz)
					
					If ForceA>.01
						LinearDamp(ode\body,.08)
					End If
					
					;If ForceA>2.5
					;	PositionEntity piv1,dGeomGetPositionX#(ode\geom),dGeomGetPositionY#(ode\geom),dGeomGetPositionZ#(ode\geom)
					;	EmitSound(snd1,piv1)
					;End If
					
					If ForceB>.01
						AngularDamp(ode\body,.08)
					End If
					
				End If
			Next
		End If
	End If
Next

End Function