/'************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * Ported to FreeBASIC by D.J.Peters                                     *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 ************************************************************************'/
#include "fbode.bi"
#include "drawstuff.bi"

' some constants
#define SIDEa 0.1f ' defth
#define SIDEb 0.5f ' width
#define SIDEc 2.0f ' height
#define MASS  10f         ' mass of a box
#define MAX_CONTACTS 8
#define GRAVITY      -0.981 ' slomotion
const as single stepsize=1f/30f ' = 30 FPS
' some globals
dim shared as dWorldID      world
dim shared as dSpaceID      world_space
dim shared as dGeomID       geomground,geombox
dim shared as dBodyID       bodybox
dim shared as dJointGroupID contactgroup
dim shared as dContact      contacts(MAX_CONTACTS-1)
dim shared as dJointID      hing
dim shared as integer draw_contacts = 1

' start simulation - set viewpoint
sub DoStart APICALL ()
  static as single xyz(2) = {-3f,0f,3f}  ' from
  static as single hpr(2) = {0f,-45f,0f} ' 45 deg pitch
  dsSetViewpoint (@xyz(0),@hpr(0))
  dMessage(0,"press:")
  dMessage(0,"      ' ' drop it again")
  dMessage(0,"      'c' togle draw contacts")
  #ifdef __FB_WIN32__
  dMessage(0,"      'T' togle texture.")
  dMessage(0,"      'S' togle shadow.")
  dMessage(0,"      'F' togle fog.")
  dMessage(0,"camera: drag the mouse")
  dMessage(0,"      left  button rotate")
  dMessage(0,"      right button move")
  dMessage(0,"      both  button up/down")
  #endif
end sub

' called when a key are pressed
sub DoCommand APICALL (cmd as integer)
  dim as string key=lcase(chr(cmd))
  select case key
    case "c":draw_contacts xor=1
    case " "
     dim as dMATRIX3    r
     dRFromAxisAndAngle(@r,1,0,0,0.0)
     dBodySetRotation(bodybox,r)
     dBodySetPosition(bodybox,0,0,4)
 
  end select
end sub
 
sub CollisionCallback cdecl (lpData as any ptr, _
                             o1     as dGeomID, _
                             o2     as dGeomID)
  static as dVector3 pos2
  static as integer  num_of_contacts
  ' our box and the ground must be involved
  dim as integer g1 = (o1 = geomground or o1 = geombox)
  dim as integer g2 = (o2 = geomground or o2 = geombox)
  if (g1 xor g2)=1 then return ' nothing to do
  ' how many contact points?
  num_of_contacts =dCollide(o1,o2,MAX_CONTACTS,@contacts(0).geom,sizeof(dContact))
  if (num_of_contacts > 0) then
    for i as integer=0 to num_of_contacts-1
      with contacts(i).surface
        .mode = dContactSlip1 _
              or dContactSlip2 _
              or dContactSoftERP _
              or dContactSoftCFM _
              or dContactApprox1
        .mu = dInfinity
        .slip1 = 0.5
        .slip2 = 0.1
        .soft_erp = 1.1  '(h*kp) / (h*kp+kd)
        .soft_cfm = 0.1  '1.0/(h*kp+kd)
      end with
      dim as dJointID c = dJointCreateContact(world,contactgroup,@contacts(i))
      dJointAttach (c,dGeomGetBody(contacts(i).geom.g1), _
                      dGeomGetBody(contacts(i).geom.g2))
      if draw_contacts then
        dsSetColor(1,1,0)
        with Contacts(i).geom
          for j as integer=0 to 2
            pos2.v(j)=.pos.v(j) + .normal.v(j)
          next
          dsDrawLine(@.pos.v(0),@pos2.v(0))
        end with
      end if
    next
  end if
end sub

' simulation loop
sub DoSimLoop APICALL (pause as integer)
  if (pause=0) then
    dSpaceCollide (world_space,NULL,@CollisionCallback)
    dWorldStep(world,1.0/60.0)
    dJointGroupEmpty (contactgroup)
  end if

  ' draw it
  static as dReal sides(2) = {SIDEa,SIDEb,SIDEc}
  dsSetTexture (DS_WOOD)
  dsSetColor(0.3,0.6,0.9)
  dsDrawBox (dBodyGetPosition(bodybox), _
             dBodyGetRotation(bodybox),@sides(0))

end sub

' 
' main 
'
dInitODE2(0)
Randomize Timer

' setup pointers to drawstuff callback functions
dim as dsFunctions fn
fn.version  = DS_VERSION
fn._start   = @DoStart
fn._step    = @DoSimLoop
fn._command = @DoCommand
fn._stop    = 0
fn._texturepath = @"textures"

' create world and set gravity
world = dWorldCreate()
dWorldSetGravity(world,0,0,GRAVITY)
' craete a contact group (faster to delete)
contactgroup = dJointGroupCreate(0)
' create an space for our objects
world_space = dHashSpaceCreate(0)
' the ground are an static object (geom only)
geomground = dCreatePlane(world_space,0,0,1,0)
' the box are an dynamic object (body and geom)
bodybox = dBodyCreate(world)
' set params for the dynamic box
dim as dMass m
dMassSetBoxTotal(@m,MASS,SIDEa,SIDEb,SIDEc)
' set the mass to the box
dBodySetMass      (bodybox,@m)
' set the position
dBodySetPosition  (bodybox,0,0,3)
' create the geom for our body box
geombox = dCreateBox(world_space,SIDEa,SIDEb,SIDEc)
' set it
dGeomSetBody (geombox,bodybox)

dim as integer w=800,h=600
' run simulation
dsSimulationLoop (0,0,w,h,@fn)
dWorldDestroy (world)
dCloseODE()

