' File: DynamicObjectTemplateTank.bas

#include once "InitGL.bi"

#define USEDEBUG
#include once "TrueAxis.bi"
#include once "SandBox.bi"

sub _AppInit_ constructor
  InitGL
  PhysicsCreateInstance
  InitSandBox
end sub

sub _AppExit_ destructor
  PhysicsDestroyInstance
end sub

type TANK_T
  public:
  declare constructor
  ' steering control
  declare property LeftTred as single
  declare property LeftTred(value as single)

  declare property RightTred as single
  declare property RightTred(value as single)
  
  declare operator cast as DynamicObject ptr

  private:
  declare static sub UpdateCB TA_CB(self as DynamicObjectTemplate ptr, fDt as single)
  as DynamicObjectTemplate ptr pObj
  as single m_LeftTred
  as single m_RightTred
  as single m_LeftSpeed
  as single m_RightSpeed
end type


constructor TANK_T
  const as single BOX_W   =  2.6
  const as single BOX_H   =  1.0
  const as single BOX_D   =  4.4
  const as single fTop    =  -0.5
  const as single fBottom =  -1
  const as single fSide   =  1.3
  const as integer nLines = 10
  dim as single Lines(nLines*2*3-1) => { _
   fSide, fTop, 2.2,  fSide, fBottom+0.1, 2.2, _
  -fSide, fTop, 2.2, -fSide, fBottom+0.1, 2.2, _
   fSide, fTop, 1.3,  fSide, fBottom    , 1.3, _
  -fSide, fTop, 1.3, -fSide, fBottom    , 1.3, _
   fSide, fTop, 0.0,  fSide, fBottom+0.1, 0.0, _
  -fSide, fTop, 0.0, -fSide, fBottom+0.1, 0.0, _
   fSide, fTop,-1.3,  fSide, fBottom    ,-1.3, _
  -fSide, fTop,-1.3, -fSide, fBottom    ,-1.3, _
   fSide, fTop,-2.2,  fSide, fBottom+0.1,-2.2, _
  -fSide, fTop,-2.2, -fSide, fBottom+0.1,-2.2}

  dim as CollisionObjectCombo ptr pCombo = CollisionObjectComboCreateNew()
  CollisionObjectComboInitialise(pCombo)

  dim as CollisionObjectConvex ptr pConvex = CollisionObjectConvexCreateNew()
  CollisionObjectConvexInitialiseAsABox(pConvex,0,0,0, BOX_W,BOX_H,BOX_D)

  CollisionObjectComboAddCollisionObject pCombo,pConvex
  CollisionObjectConvexRelease pConvex

  dim as CollisionObjectLineList ptr pLineList = CollisionObjectLineListCreateNew()
  CollisionObjectLineListInitialise pLineList,@Lines(0),nLines

  CollisionObjectComboAddCollisionObject pCombo,pLineList
  CollisionObjectLineListRelease pLineList

  pObj = DynamicObjectTemplateCreateNew()
  DynamicObjectTemplateInitialise pObj,pCombo,@UpdateCB
  CollisionObjectComboRelease pCombo

  'DynamicObjectSetMass pObj,1000
  DynamicObjectSetCenterOffset pObj, 0,-0.4,0
  DynamicObjectSetPosition pObj,0,5,0
  PhysicsAddDynamicObject pObj

  DynamicObjectSetUserData pObj,@this
end constructor

operator TANK_T.cast as DynamicObject ptr
  operator = pObj
end operator

sub TANK_T.UpdateCB TA_CB (self as DynamicObjectTemplate ptr, fDt as single)
  ' A highly simplified update for a treaded vehicle.
  const as single fSuspensionSpringConstant  = 9000.0
  const as single fSuspensionDampingConstant = fSuspensionSpringConstant*0.7
  const as single fLongitudinalFriction      = 1.5
  const as single fLateralFriction           = 1.0
  const as single fEnginTorque               = 30.0
  const as single fInvTreadInertia           = 1.0 / 30.0
  const as single fBrakeTorque               = 30.0
  const as single fMaxSpeed                  = 5.0
  const as single fMaxFrictionMult           = 2.0
  const as single fAirResistance             = 10.0

  dim as TANK_T ptr pThis = DynamicObjectGetUserData(self)

  dim as integer bLeftBrake
  dim as integer bRightBrake

  ' automatic breaking
  if abs(pThis->m_LeftTred)<0.005 then
    pThis->m_LeftTred=0.0
    bLeftBrake = 1
  end if

  if abs(pThis->m_RightTred)<0.005 then
    pThis->m_RightTred=0.0
    bRightBrake = 1
  end if



  ' save current physics frame
  dim as TA_MFrame curFrame
  DynamicObjectGetFrame self,curFrame

  ' Need run suspension and friction calculations at a higher frame rate for stability.
  const as single TARGET_FRAME_TIME = 1.0 / 300.0
  dim as integer nSteps = int(fDt / TARGET_FRAME_TIME)
  if (nSteps < 1) then nSteps = 1
  dim as single fStepTime = fDt / nSteps
  dim as single fInvStepTime = 1.0 / fStepTime

  for i as integer=1 to nSteps
    DynamicObjectAssertForceAndTorqueCleared self
    DynamicObjectAccumulateGravity self
    ' engine torque
    pThis->m_LeftSpeed  += pThis->LeftTred  * fStepTime * fEnginTorque
    pThis->m_RightSpeed += pThis->RightTred * fStepTime * fEnginTorque

    ' Wind resistance.
    dim as TA_Vec3 v3LinearVelocity : DynamicObjectV3GetLinearVelocity self,v3LinearVelocity
    Vec3Mul v3LinearVelocity,-fAirResistance * Vec3GetMagnitude(v3LinearVelocity)
    DynamicObjectV3AccumulateLinearForce self,v3LinearVelocity

    ' breaking
    if (bLeftBrake) then
      if (pThis->m_LeftSpeed > 0.0) then
        pThis->m_LeftSpeed -= fStepTime * fBrakeTorque
        if (pThis->m_LeftSpeed < 0.0) then pThis->m_LeftSpeed = 0.0
      elseif (pThis->m_LeftSpeed < 0.0) then
        pThis->m_LeftSpeed += fStepTime * fBrakeTorque
        if (pThis->m_LeftSpeed > 0.0) then pThis->m_LeftSpeed = 0.0
      end if
    elseif (pThis->m_LeftSpeed > fMaxSpeed) then
      pThis->m_LeftSpeed=fMaxSpeed
    elseif (pThis->m_LeftSpeed <-fMaxSpeed) then
       pThis->m_LeftSpeed=-fMaxSpeed
    end if

    if (bRightBrake) then
      if (pThis->m_RightSpeed > 0.0) then
        pThis->m_RightSpeed -= fStepTime * fBrakeTorque
        if (pThis->m_RightSpeed < 0.0) then pThis->m_RightSpeed = 0.0
      elseif (pThis->m_RightSpeed < 0.0) then
        pThis->m_RightSpeed += fStepTime * fBrakeTorque
        if (pThis->m_RightSpeed > 0.0) then pThis->m_RightSpeed = 0.0
      end if
    elseif (pThis->m_RightSpeed > fMaxSpeed) then
      pThis->m_RightSpeed=fMaxSpeed
    elseif (pThis->m_RightSpeed <-fMaxSpeed) then
       pThis->m_RightSpeed=-fMaxSpeed
    end if
    

    dim as integer nLines=DynamicObjectGetNumLineCollisions(self)
    dim as integer nCollisions=0
    dim as integer nLine=0
    dim as TA_Vec3 v3WheelForward 
    dim as TA_Vec3 v3WheelRight
    dim as TA_Vec3 v3Impulse
    dim as TA_Vec3 tmp 
    dim as single  fNormalForce
    dim as single  fLongitudinalForce
    dim as single  fLateralForce
    dim as single  fFrictionMult

    for nLine=0 to nLines-1
      dim as single fIntersectionDepth = DynamicObjectLineCollisionsGetIntersectionDepth(self,nLine)
      ' collision ?
      if (fIntersectionDepth>0.0) then
        nCollisions+=1
        dim as DynamicObject ptr pObj = DynamicObjectLineCollisionsGetDynamicObject(self,nLine)
        dim as integer nAttribute     = DynamicObjectLineCollisionsGetAttribute(self,nLine)
        dim as TA_Vec3 v3Position     : DynamicObjectLineCollisionsV3GetWorldPosition self,nLine,v3Position
        dim as TA_Vec3 v3Normal       : DynamicObjectLineCollisionsV3GetNormal   self,nLine,v3Normal

        ' do velocity damping
        dim as TA_Vec3 v3Velocity
        DynamicObjectV3GetVelocityAtWorldPosition(self,v3Position,v3Velocity)
        if pObj andalso PhysicsObjectAddedToPhysics(pObj) then
          if DynamicObjectIsInfiniteMass(pObj)=0 andalso DynamicObjectIsMovementDisabled(pObj)=0 then
            dim as TA_Vec3 v3ObjVelocity
            DynamicObjectV3GetVelocityAtWorldPosition pObj,v3Position,v3ObjVelocity
            Vec3Sub v3Velocity,v3ObjVelocity
          end if
        end if

        ' find the veloity at the point of collision along the ground
        dim as single fSeparatingSpeed = Vec3GetDotWith(v3Normal,v3Velocity)
        Vec3MulFrom tmp,v3Normal,fSeparatingSpeed
        Vec3Sub v3Velocity,tmp

        if fSeparatingSpeed>1.0 then
          fSeparatingSpeed=1.0
        elseif fSeparatingSpeed<-1.0 then
          fSeparatingSpeed=-1.0
        end if

        ' do spring force
        fNormalForce = (1.0 - fIntersectionDepth) * fSuspensionSpringConstant
        fNormalForce-= fSeparatingSpeed * fSuspensionDampingConstant

        ' the wheel can push but never pull.
        if (fNormalForce>0.0) then
          ' calculate the forward direction of the wheel along the ground
          DynamicObjectGetFrameRotationV3Z self,v3WheelForward
          Vec3MulFrom tmp,v3Normal,Vec3GetDotWith(v3Normal,v3Velocity)
          Vec3Sub v3WheelForward,tmp

          dim as single fMag = Vec3GetMagnitude(v3WheelForward)
          if fMag < 0.001 then continue for
          ' normalise
          Vec3Mul v3WheelForward,1.0/fMag
          
          ' longitudinal force
          fFrictionMult = fNormalForce * fLongitudinalFriction '* SurfaceAttributeGetCollisionFriction(nAttribute)
          if (fFrictionMult > fInvStepTime * fMaxFrictionMult) then 
            fFrictionMult = fInvStepTime * fMaxFrictionMult
          end if
          fLongitudinalForce = -sgn(Vec3GetDotWith(v3WheelForward,v3Velocity)) * fFrictionMult
          ' apply driving forces.
          if (nLine and 1) then
            fLongitudinalForce  += pThis->m_LeftSpeed  * fFrictionMult
            pThis->m_LeftSpeed  -= fLongitudinalForce  * fInvTreadInertia * fStepTime
          else
            fLongitudinalForce  += pThis->m_RightSpeed * fFrictionMult
            pThis->m_RightSpeed -= fLongitudinalForce  * fInvTreadInertia * fStepTime
          end if

          ' lateral force direction along ground to the side of the wheel.
          Vec3CrossFrom v3WheelRight,v3Normal,v3WheelForward
          dim as single fSlipAngle
          if Vec3GetMagnitudeSqrd(v3Velocity) > 0.05 then
            fSlipAngle = atan2(Vec3GetDotWith(v3WheelRight,v3Velocity), abs(Vec3GetDotWith(v3WheelForward,v3Velocity)))
          else
            fSlipAngle = sgn(Vec3GetDotWith(v3WheelRight,v3Velocity))
          end if
          fFrictionMult = fNormalForce * fLateralFriction '* SurfaceAttributeGetCollisionFriction(nAttribute)
          if (fFrictionMult > fInvStepTime * fMaxFrictionMult) then 
            fFrictionMult = fInvStepTime * fMaxFrictionMult
          end if
          fLateralForce = -fSlipAngle * fFrictionMult

          ' caclulate force and accumulate
          dim as TA_Vec3 v3Force ' = v3a*a + v3b*b +v3c*c
          Vec3MulAdd3 v3Force, v3Normal      ,fNormalForce, _
                               v3WheelRight  ,fLateralForce, _
                               v3WheelForward,fLongitudinalForce
          
          if pObj andalso PhysicsObjectAddedToPhysics(pObj)<>0 then
            if DynamicObjectIsInfiniteMass(pObj)=0 andalso  DynamicObjectIsMovementDisabled(pObj)=0 then
              PhysicsSetDynamicObjectIsMoving(pObj)
              Vec3MulFrom v3Impulse,v3Force,-fStepTime
              Vec3Mul v3Impulse,0.1
              DynamicObjectV3ApplyImpulse pObj,v3Impulse,v3Position
            end if
          end if

          DynamicObjectV3AccumulateForceAndTorque self,v3Force,v3Position

        end if ' fNormalForce > 0.0
      end if ' fIntersectionDepth > 0.0
    next ' next wheel

    DynamicObjectApplyForceAndTorqueToVelocities self,fStepTime
    DynamicObjectClearForceAndTorque             self
    DynamicObjectApplyVelocityToNextFrame        self,fStepTime

    if nCollisions>0 then 
      for nLine=0 to nLines-1
        DynamicObjectAdjustLineCollisionFromNextFrame self,nLine
      next
    end if
    DynamicObjectApplyNextFrame self
  next ' next super sample

  ' set old physics frame.
  DynamicObjectSetFrameDirect self,curFrame
  DynamicObjectUpdateWorldSpaceInertialTensor self

  ' set velocities based on how much the thank moved during the above supersampled loop
  if nSteps<3 then
    DynamicObjectApplyVelocityToNextFrame(self,fDt)
  else
    DynamicObjectApplyVelocityFromNextFrame(self,fDt)
  end if
  ' reset line collisions for next update
  DynamicObjectClearLineIntersections self
end sub

property TANK_T.LeftTred as single
  property = m_LeftTred
end property
property TANK_T.LeftTred(value as single)
  m_LeftTred=value
  if abs(m_LeftTred)>1.0 then m_LeftTred=sgn(m_LeftTred)
end property

property TANK_T.RightTred as single
  property = m_RightTred
end property
property TANK_T.RightTred(value as single)
  m_RightTred=value
  if abs(m_RightTred)>1.0 then  m_RightTred=sgn(m_RightTred)
end property


'
' main
'
const as integer NTHINGS=1000
dim as DynamicObject ptr thingss(NTHINGS-1)
PhysicsSetupSimulation(NTHINGS+10)

flip
 
for i as integer=1 to NTHINGS
  dim as single w,h,d,x,y,z,rx,ry,rz
  if i mod 10=0 then WindowTitle "create object(" & i & ")"
  thingss(i) = DynamicObjectCreateNew()
  if i mod 30=0 then
    h=1+rnd*2:d=h/2
    DynamicObjectInitialiseAsACylinder thingss(i),0,h/2,0, 0,-h/2,0, d
  else
    w=.1+rnd*2 : h=.1+rnd*0.2 : d=.1+rnd*3
    DynamicObjectInitialiseAsABox thingss(i),0,0,0,w,h,d
  end if

  PhysicsAddDynamicObject thingss(i)
  x=rnd2*SandBoxSize/8 : z=rnd2*SandBoxSize/8
  ' get y position 
  if PhysicsTestLineForCollision(x, 50,z, x,-50,z, ALL_OBJECTS) then
    'if CollisionOccurred() then 
    CollisionGetPosition  x,y,z
  end if
  DynamicObjectSetPosition thingss(i),x,y+h,z
  DynamicObjectSetRestTimeMultiplier thingss(i),5
  DynamicObjectSetMass thingss(i),1
  for j as integer=1 to 50
    PhysicsUpdate(1.0/30.0)
  next
  DynamicObjectSetToResting thingss(i)
next 

dim as TANK_T tank
tank.LeftTred =.5
tank.RightTred=.7

PhysicsAddStaticGround

WindowTitle "use: LEFT,RIGHT,UP and DOWN"

dim as single cx=50,cy=50,cz=50,l=0.85,r=1
dim as integer frames,key
' render and simulation loop
while key<>K_ESCAPE andalso key<>K_WINDOW_CLOSE
  key=GetKeyCode()
  if key then
    select case key
    case K_UP
      if l>r then r=l else l=r
      l+=0.01
      r+=0.01
    case K_DOWN
      if l>r then r=l else l=r
      l-=0.01
      r-=0.01
    case K_LEFT
      l+=0.01
      r-=0.01
    case K_RIGHT
      l-=0.01
      r+=0.01
    end select
    if l<-1 then 
      l=-1
    elseif l>1 then
      l=1
    end if
    if r<-1 then 
      r=-1
    elseif r>1 then
      r=1
    end if
    tank.LeftTred =l
    tank.RightTred=r
    WindowTitle "LeftTred " &  tank.LeftTred & " RightTred " & tank.RightTred
    PhysicsSetDynamicObjectIsMoving tank
  end if

  ' one step in simulation
  PhysicsUpdate(1.0/75.0)
  if frames mod 3=0 then
    glClear GL_COLOR_BUFFER_BIT or GL_DEPTH_BUFFER_BIT
    glLoadIdentity
    dim as TA_Vec3 camSrc : camSrc.y=5 : camSrc.z=-10
    DynamicObjectV3Transform tank,camSrc,camSrc
    PhysicsTestLineForCollision(camSrc.x, 50,camSrc.z, _
                                camSrc.x,-50,camSrc.z, STATIC_OBJECTS)
    if CollisionOccurred() then CollisionGetPosition  camSrc.x,camSrc.y,camSrc.z
    dim as TA_Vec3 camDst
    DynamicObjectV3GetPosition tank,camDst
    camSrc.y+=5
    cx+=(camSrc.x-cx)*0.1:cy+=(camSrc.y-cy)*0.1:cz+=(camSrc.z-cz)*0.1
    LookAt cx,cy,cz, _
           camDst.x,camDst.y+2,camDst.z, _
           0,1,0
    ' draw all objects (only in debug mode)
    PhysicsRender()
    flip
  end if
  Sleep(10)
  frames+=1
wend


