Tokamak And Ode

Blitz3D Forums/Blitz3D Programming/Tokamak And Ode

Sweenie
 #1
Here is the spring & damping system I use.
Check the function UpdateSpring.
Const FPS=60

Graphics3D 800,600,0,2
SetBuffer BackBuffer()

WireFrame False

camera = CreateCamera()
PositionEntity camera,0,3,-9

TOKSIM_CreateSimulator(0,-9.8,0)

SeedRnd MilliSecs()  

Ground = CreateCube()
ScaleMesh Ground,0.5,0.5,0.5
ScaleEntity Ground,100,10,100
EntityColor Ground,Rnd(100,230),Rnd(100,230),Rnd(100,230) 
PositionEntity Ground,0,-5,0
Ground_AB = TOKAB_Create()
TOKAB_AddBox Ground_AB,100.0,10.0,100.0
TOKAB_SetPosition Ground_AB,0.0,-5.0,0.0

Body1_Mesh = CreateSphere()
ScaleMesh Body1_Mesh ,0.5,0.5,0.5
EntityColor Body1_Mesh ,Rnd(100,230),Rnd(100,230),Rnd(100,230) 
Body1_RB = TOKRB_Create()
TOKRB_AddSphere Body1_RB,1.0
TOKRB_SetPosition Body1_RB,0.0,10,0.0
TOKRB_SetMass Body1_RB,1.0
TOKRB_SetSphereInertiaTensor Body1_RB,1.0,1.0

Body2_Mesh = CreateSphere()
ScaleMesh Body2_Mesh ,0.5,0.5,0.5
EntityColor Body2_Mesh ,Rnd(100,230),Rnd(100,230),Rnd(100,230) 
Body2_RB = TOKRB_Create()
TOKRB_AddSphere Body2_RB,1.0
TOKRB_SetPosition Body2_RB,0.0,5.0,0.0
TOKRB_SetMass Body2_RB,1.0
TOKRB_SetSphereInertiaTensor Body2_RB,1.0,1.0

SlideJoint = TOKJOINT_Create(2,Body1_RB,Body2_RB)
TOKJOINT_SetType SlideJoint,4 ; Slidejoint
TOKJOINT_SetPositionAndRotationWorld SlideJoint,0,7.5,0,0,0,0 
TOKJOINT_Enable SlideJoint,True

Centerpivot = CreatePivot()

light=CreateLight()
PositionEntity light,255,255,-115
PointEntity light,Centerpivot

period=1000/FPS
time=MilliSecs()-period

; MainLoop
While Not KeyHit(1)

	Repeat
		elapsed=MilliSecs()-time
	Until elapsed

	ticks=elapsed/period
	tween#=Float(elapsed Mod period)/Float(period)

	For k=1 To ticks
		time=time+period
		If k=ticks Then CaptureWorld

		TOKSIM_Advance(1.5/FPS,1)


	UpdateWorld

   PositionEntity Body1_Mesh,TOKRB_GetX(Body1_RB),TOKRB_GetY(Body1_RB),TOKRB_GetZ(Body1_RB) 
   RotateEntity Body1_Mesh,TOKRB_GetPitch(Body1_RB),TOKRB_GetYaw(Body1_RB),TOKRB_GetRoll(Body1_RB),False
   PositionEntity Body2_Mesh,TOKRB_GetX(Body2_RB),TOKRB_GetY(Body2_RB),TOKRB_GetZ(Body2_RB) 
   RotateEntity Body2_Mesh,TOKRB_GetPitch(Body2_RB),TOKRB_GetYaw(Body2_RB),TOKRB_GetRoll(Body2_RB),False

;   updatespring
	UpdateSpring Body1_RB,Body2_RB,50,1.0,4.0

	;Prevent it from falling over.
	TOKRB_SetAngularMomentum Body1_RB,TOKRB_GetAngularMomentumX#(Body1_RB)*0.9,TOKRB_GetAngularMomentumY#(Body1_RB),TOKRB_GetAngularMomentumZ#(Body1_RB)*0.9

	Next


	
If KeyHit(57) Then
  TOKRB_ApplyImpulse Body1_RB,0,15,0 
EndIf

	RenderWorld tween

Text 0,10,"Render Time:"+Str(elapsed)+ " milliseconds"
	Flip False

Wend

TOKSIM_DestroySimulator()

End


Function UpdateSpring(RB1,RB2,Stiffness#,Damping#,SpringLength#)

;Add spring force
diffX#=TOKRB_GetX#(RB1) - TOKRB_GetX#(RB2)
diffY#=TOKRB_GetY#(RB1) - TOKRB_GetY#(RB2)
diffZ#=TOKRB_GetZ#(RB1) - TOKRB_GetZ#(RB2)
diffLen#=Sqr(diffX#*diffX#+diffY#*diffY#+diffZ#*diffZ#)
Displacement#=diffLen#-SpringLength#
nx#=diffx#/diffLen#
ny#=diffy#/diffLen#
nz#=diffz#/diffLen#
fx#=nx#*(Displacement#*Stiffness#)
fy#=ny#*(Displacement#*Stiffness#)
fz#=nz#*(Displacement#*Stiffness#)

;Add damping force
Vx#=TOKRB_GetVelocityX#(RB1)-TOKRB_GetVelocityX#(RB2)
Vy#=TOKRB_GetVelocityY#(RB1)-TOKRB_GetVelocityY#(RB2)
Vz#=TOKRB_GetVelocityZ#(RB1)-TOKRB_GetVelocityZ#(RB2)
speed#=DotProduct#(nx#,ny#,nz#,Vx#,Vy#,Vz#)
dampX#=nx#*speed#*Damping#
dampY#=ny#*speed#*Damping#
dampZ#=nz#*speed#*Damping#

fx#=fx#+dampX#
fy#=fy#+dampY#
fz#=fz#+dampZ#


TOKRB_SetForce RB1,-fx#,-fy,-fz#
TOKRB_SetForce RB2,fx#,fy,fz#

;Stop

End Function



Function DotProduct#(x1#,y1#,z1#,x2#,y2#,z2#)
	DProd#=((x1*x2)+(y1*y2)+(z1*z2))
	Return DProd#
End Function