[bb] Rigid Body Simulation by Sweenie [ 1+ years ago ]

Started by BlitzBot, June 29, 2017, 00:28:42

Previous topic - Next topic

BlitzBot

Title : Rigid Body Simulation
Author : Sweenie
Posted : 1+ years ago

Description : This is the beginning of a simple Rigid Body simulation in Blitz.
No collisions so far.(So the difficult part is yet to be done)


Code :
Code (blitzbasic) Select
Graphics3D 800,600,0,2
SetBuffer BackBuffer()

camera = CreateCamera()
PositionEntity camera,0,0,-10

WireFrame True

Global Cprod_x#
Global Cprod_y#
Global Cprod_z#
Const NEARZERO# = 0.0001

;RigidBody Type

Type RigidBody

 Field Mass#

 Field Com_x# ; Center Of Mass
 Field Com_y#
 Field Com_z#
 Field Linearvel_x# ; Linear Velocity
 Field Linearvel_y#
 Field Linearvel_z#
 Field Linearacc_x# ; Linear Acceleration
 Field Linearacc_y#
 Field Linearacc_z#
 Field Forcevec_x# ; Force Vector
 Field Forcevec_y#
 Field Forcevec_z#
 Field Forceloc_x# ; Force Location
 Field Forceloc_y#
 Field Forceloc_z#

 Field Rotangle_x# ; Rotation Angle (Orientation)
 Field Rotangle_y#
 Field Rotangle_z#
 Field Angularvel_x# ; Angular Velocity
 Field Angularvel_y#
 Field Angularvel_z#
 Field Angularacc_x# ; Angular Acceleration
 Field Angularacc_y#
 Field Angularacc_z#
 Field Inertia_x# ; Rotational Inertia
 Field Inertia_y#
 Field Inertia_z#
 Field Torque_x# ; eh... Just Torque ;)
 Field Torque_y#
 Field Torque_z#

 Field Coeff# ; Coefficient of restitution / "Bounce Factor" 0.0 = Not elastic, 1.0 = VERY elastic, 1.1 = Too Elastic ;)
 Field Friction# ; * Not implemented yet *
 Field LinearDamping#
 Field AngularDamping#

End Type


RB.RigidBody = New RigidBody
RBMass#=1.0
RBInertia_x#=0.4
RBInertia_y#=0.4
RBInertia_z#=0.4
RBLinearDamping# = 0.001
RBAngularDamping# = 0.01

RB_Mesh = CreateSphere()

While Not KeyHit(1)

UpdateBody RB,0.02

PositionEntity RB_Mesh,RBCom_x#,RBCom_y#,RBCom_z#
RotateEntity RB_Mesh,RBRotangle_x#,-RBRotangle_y#,RBRotangle_z#

If KeyDown(205) Then ; Right
 RBForcevec_x# = 1.0
 RBForcevec_y# = 0.0
 RBForcevec_z# = 0.0
 RBForceloc_x# = 0.0
 RBForceloc_y# = 25.0
 RBForceloc_z# = -25.0
ElseIf KeyDown(203) Then ; Left
 RBForcevec_x# = -2.0
 RBForcevec_y# = 0.0
 RBForcevec_z# = 0.0
 RBForceloc_x# = 0.0
 RBForceloc_y# = 15.0
 RBForceloc_z# = -5.0
Else
 RBForcevec_x# = 0.0
 RBForcevec_y# = 0.0
 RBForcevec_z# = 0.0
EndIf


RenderWorld

Flip

Wend
End


Function UpdateBody(Body.RigidBody,dt#)

 ; *** Linear Dynamics ***

 ;Linear Acceleration
 If BodyMass#>0 Then
  BodyLinearacc_x# = BodyForcevec_x# / BodyMass#
  BodyLinearacc_y# = BodyForcevec_y# / BodyMass#
  BodyLinearacc_z# = BodyForcevec_z# / BodyMass#
 EndIf

 ;Linear Velocity
 BodyLinearvel_x# = BodyLinearvel_x# + BodyLinearacc_x# * dt#
 BodyLinearvel_y# = BodyLinearvel_y# + BodyLinearacc_y# * dt#
 BodyLinearvel_z# = BodyLinearvel_z# + BodyLinearacc_z# * dt#
 
 ;Center Of Mass
 BodyCom_x# = BodyCom_x# + BodyLinearvel_x# * dt#
 BodyCom_y# = BodyCom_y# + BodyLinearvel_y# * dt#
 BodyCom_z# = BodyCom_z# + BodyLinearvel_z# * dt#
 

 ; *** Rotational Dynamics ***

 ;Torque
 CrossProduct BodyForceloc_x#,BodyForceloc_y#,BodyForceloc_z#,BodyForcevec_x#,BodyForcevec_y#,BodyForcevec_z#
 BodyTorque_x#=Cprod_x#
 BodyTorque_y#=Cprod_y#
 BodyTorque_z#=Cprod_z#

 ;Angular Acceleration
 BodyAngularacc_x# = BodyTorque_x# / BodyInertia_x#
 BodyAngularacc_y# = BodyTorque_y# / BodyInertia_y#
 BodyAngularacc_z# = BodyTorque_z# / BodyInertia_z#

 ;Angular Velocity
 BodyAngularvel_x# = BodyAngularvel_x# + BodyAngularacc_x# * dt#
 BodyAngularvel_y# = BodyAngularvel_y# + BodyAngularacc_y# * dt#
 BodyAngularvel_z# = BodyAngularvel_z# + BodyAngularacc_z# * dt#

 ;Angles of rotation / Orientation
 BodyRotangle_x# = BodyRotangle_x# + BodyAngularvel_x# * dt#
 BodyRotangle_y# = BodyRotangle_y# + BodyAngularvel_y# * dt#
 BodyRotangle_z# = BodyRotangle_z# + BodyAngularvel_z# * dt#

 ;Damp Linear & Angular Velocity
 BodyLinearvel_x#=BodyLinearvel_x#*(1.0-BodyLinearDamping#)
 BodyLinearvel_y#=BodyLinearvel_y#*(1.0-BodyLinearDamping#)
 BodyLinearvel_z#=BodyLinearvel_z#*(1.0-BodyLinearDamping#)
 BodyAngularvel_x#=BodyAngularvel_x#*(1.0-BodyAngularDamping#)
 BodyAngularvel_y#=BodyAngularvel_y#*(1.0-BodyAngularDamping#)
 BodyAngularvel_z#=BodyAngularvel_z#*(1.0-BodyAngularDamping#)

 ;Reset Velocities If Near Zero Value / *Hack* ;) Not necessary but prevents jittering when Body is considered not moving.
 If Abs(BodyLinearvel_x#)<NEARZERO# Then BodyLinearvel_x#=0.0
 If Abs(BodyLinearvel_y#)<NEARZERO# Then BodyLinearvel_y#=0.0
 If Abs(BodyLinearvel_z#)<NEARZERO# Then BodyLinearvel_z#=0.0
 If Abs(BodyAngularvel_x#)<NEARZERO# Then BodyAngularvel_x#=0.0
 If Abs(BodyAngularvel_y#)<NEARZERO# Then BodyAngularvel_y#=0.0
 If Abs(BodyAngularvel_z#)<NEARZERO# Then BodyAngularvel_z#=0.0


End Function

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

Function CrossProduct(x1#,y1#,z1#,x2#,y2#,z2#)
Cprod_x#=(y1#*z2#)-(z1#*y2#)
Cprod_y#=(z1#*x2#)-(x1#*z2#)
Cprod_z#=(x1#*y2#)-(y1#*x2#)
End Function


Comments : none...