JV-ODE Physics Thread 4

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

VIP3R : I've changed the magnetism function a bit to check collisions with magnetised objects. I'm showing level of magnetism by turning the objects blue. If the magnet is surrounded by magnetised (blue) objects, other objects hitting those blue objects also become magnetised and turn blue.

I cycle through each of the collisions and for each collision geom, cycle through the array of objects to find that object with a matching geom.

Only it doesn't work, and I can't find why.
Include "JV-ODE.bb"

Graphics3D 800,600,0,2

Global WorldERP#=0.1			; ### World Error Correction
Global WorldFriction#=0			; ### World Friction

Global SpaceInfo=0

Global Pointer
Global PlaneGeom
Global magnet.ODEGeom, magnet_on
Const NUM_OBJECTS=12

Type ODEGeom
	Field body
	Field geom
	Field mesh
	Field magnetised
End Type
Dim clutter.ODEGeom(NUM_OBJECTS)
; ###################################################################################################

; ### Setup ODE

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

Global GroundSpace=dHashSpaceCreate(Space)
Global ObjSpace=dHashSpaceCreate(Space)
Global ContactGroup=dJointGroupCreate(0)

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

dContactSetMode(dContactSlip1)
dContactSetMu(WorldFriction)

; ### Create light
Light=CreateLight()
RotateEntity Light,45,-90,0
LightColor Light,255,255,255
AmbientLight 130,130,130

; ### Create camera
pivot=CreatePivot()
Camera=CreateCamera(pivot)
PositionEntity Camera,-0,40,-80
RotateEntity Camera,30,0,0
CameraClsColor Camera,0,0,0
CameraRange Camera,1,1000
CameraZoom camera,2

; ### Create plane
PlaneGeom=dCreatePlane(GroundSpace,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

; ### Create some objects
	GRAVITY_ENABLED=True
	SIZE#=2.0
	CYLINDER_RADIUS#=1.7319
	DENSITY#=1.0
	FRICTION#=10.0
	gamma#=0.5

; Change Cube size to match Sphere's volume
x#=3.0
y#=5.0
z#=3.0
magnet.ODEGeom=New ODEGeom
	magnet\geom=dCreateBox(Space,x,y,z)
	dGeomSetPosition (magnet\geom,0,2.5,0)
	dGeomContactSetMu(magnet\geom,0.0)
	magnet\mesh=CreateCube()
	ScaleMesh magnet\mesh,x/2,y/2,z/2
	PositionEntity magnet\mesh,0,2.5,0
	EntityColor magnet\mesh,255,0,0
	magnet\magnetised=1
;	dGeomContactSetSoftCFM(magnet\geom,0.001)
	
SeedRnd MilliSecs() 
For n=1 To NUM_OBJECTS
	clutter(n)=New ODEGeom
;	r=Rand(1,2) ; Random objects
;	r = 1 ; Create all objects as spheres
	r = 2 ; Create all objects as boxes
	x#=Rnd(1,3)
	y#=Rnd(1,3)
	z#=Rnd(1,3)	
	Select r
		Case 1
			clutter(n)\body=dBodyCreate(World)
			clutter(n)\geom=dCreateSphere(Space,x)
			dGeomSetBody(clutter(n)\geom,clutter(n)\body)
			dGeomContactSetMu(clutter(n)\geom,0.0)
			mass=dMassCreate()
				dMassSetSphere(mass,DENSITY,x)
				dBodySetMass(clutter(n)\body,mass)
				dMassDestroy(mass)
			clutter(n)\mesh=CreateSphere()
			ScaleMesh clutter(n)\mesh,x,x,x	
		Case 2
;			x=x*2 :	y=y*2 : 	z=z*2		; Double box sizes
			clutter(n)\body=dBodyCreate(World)
			clutter(n)\geom=dCreateBox(Space,x,y,z)
			dGeomSetBody(clutter(n)\geom,clutter(n)\body)
			dGeomContactSetMu(clutter(n)\geom,0.0)
			mass=dMassCreate()
				dMassSetBox(mass,DENSITY,x,y,z)
				dBodySetMass(clutter(n)\body,mass)
				dMassDestroy(mass)
			clutter(n)\mesh=CreateCube()
			ScaleMesh clutter(n)\mesh,x/2,y/2,z/2	
	End Select
	dBodySetRotation(clutter(n)\body,Rand(-90,90),Rand(-90,90),Rand(-90,90))
	dBodySetPosition(clutter(n)\body,Rand(-30,30),Rand(3,5),Rand(-30,30))
	EntityColor clutter(n)\mesh,255,255-((n-1)*(255/NUM_OBJECTS)),255-(n*(255/NUM_OBJECTS))
Next

time=0
magnet_on=False
While Not KeyHit(1)

	time=advance_simulation(time)
	If KeyHit(57)
		magnet_on=True
	ElseIf KeyDown(203)
		TurnEntity pivot,0,0.5,0
	ElseIf KeyDown(205)
		TurnEntity pivot,0,-0.5,0
	EndIf
	If KeyDown(200)
		MoveEntity camera,0,0,1
	ElseIf KeyDown(208)
		MoveEntity camera,0,0,-1
	EndIf
	PointEntity camera,magnet\mesh
	UpdateWorld
	RenderWorld
	
	y=15
	Text 20,y,clutter(1)\magnetised : y=y+15
	Text 20,y,clutter(2)\magnetised : y=y+15
	Text 20,y,clutter(3)\magnetised : y=y+15
	Text 20,y,clutter(4)\magnetised : y=y+15
	Text 20,y,clutter(5)\magnetised : y=y+15
	Text 20,y,clutter(6)\magnetised : y=y+15
	Text 20,y,clutter(7)\magnetised : y=y+15
	Text 20,y,clutter(8)\magnetised : y=y+15
	Text 20,y,clutter(9)\magnetised : y=y+15
	Text 20,y,clutter(10)\magnetised : y=y+15
	Text 20,y,clutter(11)\magnetised : y=y+15
	Text 20,y,clutter(12)\magnetised : y=y+15

	Flip

Wend

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

End

Function advance_simulation(t%)
	; Advances the simluation, incrementing frame count
	dSpaceCollide(Space,World,ContactGroup)

	;#Region Apply Forces
	objects_stopped=True
	;#End Region
	For n=1 To NUM_OBJECTS
		PositionEntity clutter(n)\mesh,dGeomGetPositionX(clutter(n)\geom),dGeomGetPositionY(clutter(n)\geom),dGeomGetPositionZ(clutter(n)\geom)
		RotateEntity clutter(n)\mesh,dGeomGetPitch(clutter(n)\geom),dGeomGetYaw(clutter(n)\geom),dGeomGetRoll(clutter(n)\geom)
		If magnet_on
			apply_magnetism(magnet,clutter(n))
		EndIf
		
		; Per collision processing
		If dBodyIsEnabled(clutter(n)\body)
			numCollisions=dGeomCountCollisions(clutter(n)\geom)
			If numCollisions>0
				; For each collision, test if with magnet
				For m=1 To numCollisions
					collision_geom=dGeomCollisionGeom(clutter(n)\geom,m)
					; Get object that is parent to collision geom
					o = 0
					Repeat
						o=o+1
						If o>NUM_OBJECTS
							Exit
						EndIf
						If clutter(o)\geom = collision_geom
							Exit
						EndIf
					Forever

					If clutter(n)\magnetised=1
						dGeomContactSetMu(clutter(n)\geom,10000)
						dGeomContactSetBounceVel(clutter(n)\geom,0)
					Else If clutter(n)\magnetised=50
						dGeomSetBody(clutter(n)\geom,0)
					EndIf
					If collision_geom=magnet\geom
						; Collided with magnetic object
						EntityColor clutter(n)\mesh,255-(clutter(n)\magnetised*5),255-(clutter(n)\magnetised*5),255
						clutter(n)\magnetised=clutter(n)\magnetised+1
					Else If o<NUM_OBJECTS
						If clutter(o)\magnetised>0
							clutter(n)\magnetised=clutter(n)\magnetised+1
						EndIf
					EndIf
				Next
			EndIf
			; Limit Linear Velocity...
;			bx#=dBodyGetLinearVelX(clutter(n)\body)
;			by#=dBodyGetLinearVelY(clutter(n)\body)
;			bz#=dBodyGetLinearVelZ(clutter(n)\body)
;			b#=Sqr((bx*bx)+(by*by)+(bz*bz))
;			If b>MAX_SPEED
;				b=MAX_SPEED/b
;				dBodySetLinearVel(temp\body,bx*b,by*b,bz*b)
;			EndIf
		EndIf
	Next

	dWorldQuickStep(World,0.15)
	dJointGroupEmpty(ContactGroup)
	Return t+1
End Function
Function apply_magnetism(magnet.ODEGeom, target.ODEGeom)
	; FUNCTION : Applies a force in direction of fan based on angle to fan
	dBodyGetPosition(target\body)
		x#=dVectorX()
		y#=dVectorY()
		z#=dVectorZ()
	dGeomGetPosition(magnet\geom)
		x#=x-dVectorX()
		y#=y-dVectorY()
		z#=z-dVectorZ()
	; (x,y,z) is direction of force. Need angle and magnitude
		mag#=Sqr(x * x + y * y + z * z)
		magnitude#=mag
		If mag>0
			x = x / mag
			y = y / mag
			z = z / mag
		Else
			x=1.0
			y=0.0
			z=0.0
		EndIf
		magnitude=magnitude*magnitude
	; (x,y,z) = normalized vector
		magnitude#=1.0/(magnitude)
		If magnitude > 0.001
			magnitude=magnitude*1000.0
;			magnitude=magnitude/(target\magnetised+1)
			x=x * magnitude#
			y=y * magnitude#
			z=z * magnitude#

;			If magnitude>500
;				magnitude=500
;				dBodyEnable(target\body)
;				dBodyAddForce (target\body,-x,-y,-z)
			If magnitude>0.01
				dBodyEnable(target\body)
				dBodyAddForce (target\body,-x,-y,-z)
			EndIf
		EndIf
End Function