JV-ODE Physics Thread 11

Blitz3D Forums/Blitz3D Userlibs/JV-ODE Physics Thread 11

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.

I'm having a bit of trouble repositioning and rotating a rigid body AFTER it has been made.

I'd rather 'recycle' Bodies, Geoms & Meshes , instead of destroying them.

The general idea is that the distance of all 25 cars is calculated.
This list is sorted. If a car is outside the maximum distance, it gets
repositioned.

Every road in the game has a number of waypoints.
Each waypoint has a number, and points towards his next Waypoint.
This generates the YAW of that Waypoint. This YAW is used to rotate the rigid body..... Only it doesn't work. :/

I've tried dbodysetrotation , dbodysetaxisangle . I even tried disabling bodies before positioning and rotating, but that didn't work either.

This is the code so far:


; ###################################################################################################
; #                                        JV-ODE Car Demo                                          #
; #                                  Code by Jim Williams (VIP3R)                                   #
; #                              Devious Codeworks - Copyright © 2008                               #
; ###################################################################################################

Include "JV-ODE.bb"

AppTitle "JV-ODE Car Demo"

Graphics3D 800,600,0,2

HidePointer 

Global CMass#=200.0				; ### Car Mass
Global WMass#=14.0				; ### Wheel Mass

Global WorldERP#=1.0			; ### World Error Correction
Global WorldFriction#=70.0		; ### World Friction

Global Torque#=24.0				; ### Joint Torque
Global SuspensionHS#=0.01		; ### Suspension Hardness/Softness (Higher=Softer - Lower=Harder)

Global Force#=0.0
Global Steer#=0.0

Global Car
Global CGeom
Global CMesh

Global CarStartY=10
Global TotalWP					; total no. of waypoints
Global Next_Waypoint			; car travels to this waypoint		
Global Dummy					; The Dummy Car 
Global cam_x#,cam_z#,cam_pitch#,cam_yaw#						
Global dest_cam_x#,dest_cam_z#,dest_cam_pitch#,dest_cam_yaw#	
 
Global 		CUBE = CreateCube()
ScaleMesh 	CUBE , 1.6 , 0.1 , 1.6
EntityColor CUBE , 255 , 0 , 0
HideEntity 	CUBE

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

Type ODEGeom
	Field body
	Field geom
	Field mesh
End Type

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

; ### Setup ODE

dInitODE()

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

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

dContactSetMode(dContactSlip1)
dContactSetMu(WorldFriction)

; ### Create Car Body

ode.ODEGeom=New ODEGeom
ode\body=dBodyCreate(World)
Car=ode\body
dBodySetPosition(ode\body,0,CarStartY,0)
dBodySetRotation(ode\body,0,0,0)
mass=dMassCreate()
dMassSetBoxTotal(mass,CMass,3,1,4)
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
EntityColor ode\mesh,40,80,255

; ### Attach Compass to Car
Global 		 CarCompass = CreateCone()
RotateMesh   CarCompass , 90 , 0 , 0 
ScaleMesh    CarCompass , 1,0.1,1
EntityColor  CarCompass , 200,10,10
EntityParent CarCompass , Cmesh
x# = EntityX#( Cmesh , 1 )
y# = EntityY#( Cmesh , 1 )
z# = EntityZ#( Cmesh , 1 )
PositionEntity CarCompass , x# , y#+1 , z# , 1

; ### Create Wheels

For count=1 To 4
	ode.ODEGeom=New ODEGeom
	ode\body=dBodyCreate(World)
	Wheel(count)=ode\body
	dBodySetPosition(ode\body,0,0,0)
	dBodySetRotation(ode\body,0,0,90)
	mass=dMassCreate()
	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()
	ScaleMesh ode\mesh,0.7,0.25,0.7
	EntityColor ode\mesh,255,255,255
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 Light

Global 			Light = CreateLight()
RotateEntity 	Light ,  50 ,-50  , 0
LightColor 		Light , 255 , 255 , 255
AmbientLight    130 , 130 , 130

; ### Create Camera

Global CameraPivot=CreatePivot()

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

; ### Create Plane

dCreatePlane    (Space,0,1,0,0)
Global 			Plane = CreatePlane()
EntityAlpha 	Plane , 1
EntityTexture   Plane,PlaneTexture()

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

SetupCar()
Make_DummyCar()
Load_Waypoints()
MoveMouse GraphicsWidth()/2,GraphicsHeight()/2

PositionEntity CameraPivot , 0 , 8 , -10 

While Not KeyHit(1)

	UpdateCar()

	UpdateGeoms()

	PTime=MilliSecs()

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

	PhysicsTime#=MilliSecs()-PTime

	UpdateWorld 
	RenderWorld

	ExampleCODE()
	CAR_AI()

	Color 100,100,255

	Text   0 ,  0 , "JV-ODE Version "+ dGetVersion()
	Text   0 , 15 , "Physics Time.. " + PhysicsTime
	Text   0 , 50 , "Force......... " + Force
	Text   0 , 65 , "Torque........ " + Torque
	Text 240 ,  0 , "Point Mouse at Waypoint"
	Text 240 , 15 , "Click [LMB] to rotate DummyCar at Waypoint's YAW"
	Text 240 , 30 , "Move Camera with [W] & [S] ,or Cursor[UP] & [DOWN]"
	Text 240 , 45 , "Press [SPACE] to reset car" 

	Color 255,255,255
	
	Flip 1

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

End Function

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

Function UpdateCar()

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

dBodyEnable(Car)

For count=1 To 4
	dJointSetHinge2Param(Joint(count),dParamVel2,Force)
	dJointSetHinge2Param(Joint(count),dParamFMax2,Torque)
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 UpdateGeoms()

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

End Function

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

Type Waypoint
	Field X,Y,Z
	Field Pivot
	Field Number
	Field Next_WP_Num
	Field Rotate_To_Next_WP
End Type

Function ExampleCODE()

; Mouse look

	PositionEntity Camera , EntityX#(camerapivot),EntityY#(camerapivot),EntityZ#(camerapivot)

	mxs=MouseXSpeed()
	mys=MouseYSpeed()
	dest_cam_yaw#=dest_cam_yaw#-mxs
	dest_cam_pitch#=dest_cam_pitch#+mys
	cam_yaw=cam_yaw+((dest_cam_yaw-cam_yaw)/5)
	cam_pitch=cam_pitch+((dest_cam_pitch-cam_pitch)/5)
	
	RotateEntity camera,cam_pitch#,cam_yaw#,0
	
	MoveMouse GraphicsWidth()/2,GraphicsHeight()/2

; Camera move
	
	If KeyDown(200) Or KeyDown(17)  Then dest_cam_z=1
	If KeyDown(208) Or KeyDown(31)  Then dest_cam_z#=-1

	cam_z=cam_z+((dest_cam_z-cam_z)/5)
	cam_x=cam_x+((dest_cam_x-cam_x)/5)

	RotateEntity camerapivot , 0 , EntityYaw#( Camera ,1) , 0 ,1
	MoveEntity camerapivot,cam_x,0,cam_z
	dest_cam_x=0 : dest_cam_z=0

	MX = MouseX()
	MY = MouseY()

; Draw Cursor
	
	Color 32,32,32
	For tx=-1 To 1
		For ty=-1 To 1
			Line mx+tx,my+ty,mx+6+tx,my+6+ty
			Line mx+tx,my+ty,mx+tx,my+8+ty
		Next
	Next
	Color 255,255,255
	Line mx,my,mx+6,my+6
	Line mx,my,mx,my+8

	CameraPick Camera,MX,MY

	W1.Waypoint = Null
	
	If Not PickedEntity()=0
		W1.WayPoint = Object.WayPoint (Int(EntityName(PickedEntity())))
	EndIf

	If W1.Waypoint <> Null
		
		Color 255 , 0 , 0
		Text MX , MY    , "Waypoint      " + W1\Number
		Text MX , MY+15 , "Next Waypoint " + W1\Next_WP_Num
		Text MX , MY+30 , "Rotation      " + W1\Rotate_To_Next_WP
		Color 255 , 255 , 255
	
		If MouseHit(1)
			PositionEntity Dummy , W1\X , W1\Y + 1 , W1\Z , 1
			RotateEntity   Dummy , 0 , W1\Rotate_To_Next_WP , 0 ,1
		
			;For count=1 To 4
			;	dBodyDisable(Wheel(count))
			;Next
			;dBodyDisable(Car)
		
			dBodySetPosition  ( Car ,W1\X , W1\Y + 20 , W1\Z)
			dBodySetRotation  ( Car, dGeomGetPitch(CGeom),dGeomGetYaw(CGeom),0)   
			dBodySetAxisAngle ( Car ,2 , 0 , W1\Rotate_To_Next_WP , 0 )
			
			;For count=1 To 4
			;	dBodyEnable(Wheel(count))
			;Next
			;dBodyEnable(Car)

			
			
			FlushMouse()
			
		EndIf
	EndIf 

	If KeyHit(57)=1
		Force=0.0
		dBodySetRotation(Car,dGeomGetPitch(CGeom),dGeomGetYaw(CGeom),0)
	End If


End Function

Function CAR_AI()

	If TotalWP => 22
	
		Select Next_Waypoint 
			Case 0 
				Temp.WayPoint = First WayPoint 
				Next_Waypoint = Temp\Number 
			
				Temp.WayPoint = Last WayPoint 
				Temp\Next_WP_Num = 1 

			Default
				For W.Waypoint = Each Waypoint 
					If W\Number = Next_Waypoint 
						Temp = W
						Exit
					EndIf
				Next 	
				
				PointEntity CarCompass , Temp\Pivot
				
				Distance = EntityDistance ( Cmesh , Temp\Pivot )
				If Distance < 5 
					Next_Waypoint = Temp\Next_WP_Num
				Else 
					Force = Force + 0.08
					If Force > 3 Then Force = 3
				End If

				CarAngle     = EntityYaw#( Cmesh      , 1 ) 
				CompassAngle = EntityYaw#( CarCompass , 1 )

				TempAngle# = AngleDifference( CarAngle , CompassAngle )
	
				If TempAngle < -5 Then TempAngle = -5
				If TempAngle >  5 Then TempAngle =  5

				Steer = -(TempAngle * 0.05)

		End Select 
	EndIf
	
End Function

Function AngleDifference( CurrentAngle , TargetAngle )
    If TargetAngle - CurrentAngle > 180 Then TargetAngle = TargetAngle - 360
    If CurrentAngle - TargetAngle > 180 Then CurrentAngle = CurrentAngle - 360
    Return CurrentAngle - TargetAngle
End Function

Function Load_Waypoints()

	Restore WPdata

	Read Total
	
	For T = 1 To Total
	
		Read nX 
		Read nY 
		Read nZ
		Read nCurrentWP
		Read nNextWP
		Read nRotation

		W.WayPoint    = New WayPoint 

		W\X 		        = nX 
		W\Y 		        = nY 
		W\Z 		        = nZ
		W\Pivot  	        = CopyEntity ( CUBE )
		W\Number 	        = nCurrentWP
		W\Next_WP_Num       = nNextWP
		W\Rotate_To_Next_WP = nRotation
		
		PositionEntity W\Pivot , W\X , W\Y , W\Z , 1
		NameEntity     W\Pivot , Str$( Handle ( W.WayPoint ))
		EntityPickMode W\Pivot , 2
		RotateEntity   W\Pivot , 0 , Rotation , 0 , 1
	Next 
	
	TotalWP = T-1 
	  	
End Function

Function Make_DummyCar()

	Dummy = CreateCube()
	ScaleMesh Dummy , 1.5 , 0.5 , 2
	EntityColor Dummy ,0,0,255

	W1 = CreateCylinder(16) 
 	ScaleMesh  W1 , 0.7,0.25,0.7
	RotateMesh W1 , 0 , 0 , 90
	EntityColor W1 ,0 ,0,255

	W2 = CopyEntity ( W1 )
	W3 = CopyEntity ( W1 )
	W4 = CopyEntity ( W1 )
	
	PositionEntity W1 ,-2 , -0.5 , +2 
	PositionEntity W2 ,+2 , -0.5 , +2 
	PositionEntity W3 ,-2 , -0.5 , -2 
	PositionEntity W4 ,+2 , -0.5 , -2 

	EntityParent W1 , Dummy
	EntityParent W2 , Dummy
	EntityParent W3 , Dummy
	EntityParent W4 , Dummy

End Function

.WPdata

Data 22
Data  0   , 0 ,  43 ,  1 ,  2 ,  28
Data -9   , 0 ,  60 ,  2 ,  3 ,  43
Data -25  , 0 ,  77 ,  3 ,  4 ,  53
Data -42  , 0 ,  90 ,  4 ,  5 ,  48
Data -81  , 0 , 125 ,  5 ,  6 ,  26
Data -104 , 0 , 173 ,  6 ,  7 , -4
Data -102 , 0 , 201 ,  7 ,  8 , -36
Data -83  , 0 , 227 ,  8 ,  9 , -48
Data -63  , 0 , 245 ,  9 , 10 , -90
Data -36  , 0 , 245 , 10 , 11 , -90
Data -2   , 0 , 245 , 11 , 12 , -110
Data  34  , 0 , 232 , 12 , 13 , -144
Data  63  , 0 , 192 , 13 , 14 , -159
Data  77  , 0 , 155 , 14 , 15 ,  178
Data  75  , 0 , 101 , 15 , 16 ,  155
Data  52  , 0 ,  52 , 16 , 17 ,  126
Data  29  , 0 ,  35 , 17 , 18 ,  104
Data -3   , 0 ,  27 , 18 , 19 ,   49
Data -27  , 0 ,  48 , 19 , 20 , -4
Data -21  , 0 , 142 , 20 , 21 , -90
Data  32  , 0 , 142 , 21 , 22 , -169
Data  51  , 0 ,  40 , 22 , 1  ,  0



Thanks! :D