' test of: hinge joint
#include once "InitGL.bi"

#define USEDEBUG
#include once "TrueAxis.bi"

sub _AppInit_ constructor
  InitGL
  PhysicsCreateInstance
end sub

sub _AppExit_ destructor
  PhysicsDestroyInstance
end sub

' create a static ground plane
PhysicsAddStaticGround(10)

var cube = DynamicObjectCreateNew()
DynamicObjectInitialiseAsACube cube, 0,0,0, 1
DynamicObjectSetPosition       cube, 0,.5,0
PhysicsAddDynamicObject        cube

var box = DynamicObjectCreateNew()
DynamicObjectInitialiseAsABox box, 0,0,0,  0.3, .05, 10
DynamicObjectSetPosition      box, 0,1.025,0
PhysicsAddDynamicObject       box

var joint = DynamicObjectAddJoint(cube,box)

PhysicsJointInitialiseHinge joint, 0,1.1,0, _ ' position
                                   0,1,0,  _ ' Y axis of rotation
                                  JointLimitDisabled, _ ' min angle or off
                                  JointLimitDisabled    ' max angle or off

PhysicsJointSetCollisionDisabledBetweenObjects joint,1

'DynamicObjectSetRestTimeMultiplier cube,0
DynamicObjectSetRestTimeMultiplier box,0
DynamicObjectSetRotationDisabled cube,1
DynamicObjectSetAllDampingDisabled box,1


dim as TA_Vec3 a,b
' render and simualtion loop
dim as integer key,frames

const as single FPS = 75.0
const as single fDt = 1.0/FPS
DynamicObjectSetAngularVelocity box,0,360*DEG2RAD,0
'DynamicObjectApplyAngularImpulse box , 0, 125,0

while key<>K_ESCAPE andalso key<>K_WINDOW_CLOSE
  key=GetKeyCode()
  dim as TA_Vec3 v3AngVel
  DynamicObjectV3GetAngularVelocity box,v3AngVel
  dim as single fSpeed=Vec3GetMagnitude(v3AngVel)
  
  if fSpeed<15.0 then
    'DynamicObjectApplyAngularImpulse box , 0, .75,0
    'DynamicObjectApplyAngularImpulse cube, 0,-.75,0
  elseif fSpeed>5.0 then
    'DynamicObjectApplyAngularImpulse box , 0, -.1,0
  end if
  ' one step in simulation
  PhysicsUpdate(fDt)

  if frames mod int(FPS/25)=0 then
    glClear GL_COLOR_BUFFER_BIT or GL_DEPTH_BUFFER_BIT
    glLoadIdentity
    glTranslatef 0,-2,-10
    ' draw all objects (only in debug mode) 
    PhysicsRender()
    flip
  end if
  sleep(1000/FPS)
  frames+=1
wend

