' 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


const NJOINTS=30

' create a static ground plane
PhysicsAddStaticGround(20)

dim as DynamicObject ptr Cubes(NJOINTS-1)
dim as PhysicsJoint  ptr Joints(NJOINTS-1)

for i as integer = 0 to NJOINTS-1
  cubes(i) = DynamicObjectCreateNew()
  DynamicObjectInitialiseAsACube cubes(i), 0,0,0, 1
  DynamicObjectSetPosition       cubes(i), -NJOINTS/2+i*1.1,2,0
  PhysicsAddDynamicObject        cubes(i)
  joints(i) = DynamicObjectAddWorldJoint(cubes(i))
  PhysicsJointInitialiseHinge joints(i), -NJOINTS/2+i*1.1,20,0, _ ' position
                                         1,0,0,  _ ' x axis of rotation
                                         JointLimitDisabled, _ ' min angle or off
                                         JointLimitDisabled    ' max angle or off
  DynamicObjectApplyLinearImpulse cubes(i),0,0,80+i*2+sin(i/NJOINTS)*50
next

dim as TA_Vec3 v,a,b

' render and simualtion loop
dim as integer key,frames
'DynamicObjectApplyAngularImpulse cube,0,0,50

'DynamicObjectApplyImpulse        cube,50,0,0, 0,2,0
'DynamicObjectSetLinearVelocity   cube,170,0,0

while key<>K_ESCAPE andalso key<>K_WINDOW_CLOSE
  key=GetKeyCode()  
  ' one step in simulation
  PhysicsUpdate(1.0/75)

  if frames mod 4=0 then

    glClear GL_COLOR_BUFFER_BIT or GL_DEPTH_BUFFER_BIT
    glLoadIdentity
    glTranslatef 0,-5,-40
    ' draw all objects (only in debug mode) 
    PhysicsRender()
    ' draw the joints as lines
    glDisable(GL_LIGHTING)
    for i as integer=0 to NJOINTS-1
      DynamicObjectV3GetPosition(cubes(i),a)
      PhysicsJointV3GetPosOnObjectB(joints(i),b)
      DynamicObjectV3GetLinearVelocity cubes(i),v
      glBegin(GL_LINES)
        glColor3fv(v)
        glVertex3fv(a)
        glVertex3fv(b)
      glEnd()
    next
    glEnable(GL_LIGHTING)
    flip
  end if
  sleep 10
  frames+=1
wend

