// Project: 3dcollisiontest1 
// Created: 21-07-31

// show all errors

SetErrorMode(2)

// set window properties
SetWindowTitle( "3dcollisiontest1" )
SetWindowSize( 1024, 768, 0 )
SetWindowAllowResize( 0 ) // allow the user to resize the window

// set display properties
scrw as integer = 1024
scrh as integer = 768
SetVirtualResolution( scrw, scrh ) // doesn't have to match the window
SetOrientationAllowed( 1, 1, 1, 1 ) // allow both portrait and landscape on mobile devices
SetSyncRate( 0, 0 ) // 30fps instead of 60 to save battery
SetScissor( 0,0,0,0 ) // use the maximum available screen space, no black borders
UseNewDefaultFonts( 1 )


Create3DPhysicsWorld()
//Debug3DPhysicsWorld()
Set3DPhysicsGravity(0,0,0)

obj as integer[2]
pobj as integer[2]
objx as float[2]
objy as float[2]
objz as float[2]
f_delta as float

SetCameraPosition(1, 0,0,-20)
SetCameraLookAt(1, 0,0,0,0)

//
CreateObjectBox(1, 1,1,1)
CreateObjectBox(2, 0.3,0.3,0.3)
SetObjectPosition(2, 0,2,0)
FixObjectToObject( 2, 1)
SetObjectColor(1, 255,0,0,255)
SetObjectColor(2, 255,0,0,255)
objx[1] = 0
objy[1] = -10
objz[1] = 0
SetObjectPosition(1, objx[1],objy[1],objz[1])

//
CreateObjectBox(3, 1,1,1)
CreateObjectBox(4, 0.3,0.3,0.3)
SetObjectPosition(4, 0,-2,0)
FixObjectToObject( 4, 3)
SetObjectColor(3, 0,255,0,255)
SetObjectColor(4, 0,255,0,255)
objx[2] = 0
objy[2] = 2
objz[2] = 0
SetObjectPosition(3, objx[2],objy[2],objz[2])

Create3DPhysicsKinematicBody(1)
Create3DPhysicsKinematicBody(3)

SetObjectShapeBox( 1 )
SetObjectShapeBox( 3 )

CreateObjectBox(5, 1,1,1)
SetObjectPosition(5, -4,0,0)

Create3DPhysicsDynamicBody(5)
SetObjectShapeBox( 5 )
SetObject3DPhysicsDamping( 5, 100.0, 100.0 )
SetObject3DPhysicsMass( 5, 100.0 )

rad as float = 2.0
CreateObjectSphere(6, rad,8,8)
SetObjectPosition(6, objx[2],objy[2],objz[2])
SetObjectTransparency(6, 1)
SetObjectAlpha(6, 0x50)

vecid as integer
vecid1 as integer
fromVec3ID as integer 
toVec3ID as integer
fromVec3ID1 as integer 
toVec3ID1 as integer

sfromVec3ID as integer 
stoVec3ID as integer

soutVec3ID as integer

rayid as integer
rayid = Create3DPhysicsRay()
rayid1 as integer
rayid1 = Create3DPhysicsRay()
srayid as integer
srayid = Create3DPhysicsRay()

fromVec3ID = CreateVector3(0,0,0)
toVec3ID = CreateVector3(0,0,0)
fromVec3ID1 = CreateVector3(0,0,0)
toVec3ID1 = CreateVector3(0,0,0)
sfromVec3ID = CreateVector3(0,0,0)
stoVec3ID = CreateVector3(0,0,0)

soutVec3ID = CreateVector3(0,0,0)

col1 as integer
col1 = MakeColor(255,255,0)
col2 as integer
col2 = MakeColor(255,0,255)

sss as string
sx as float
sy as float
sx1 as float
sy1 as float
h as float
sret as integer
t as float

sss = ''
do
	f_delta = GetFrameTime()
	print('Use arrow up/left/right/down keys to move')
    Print( ScreenFPS() )
     
    if GetRawKeyState(37) = 1
       RotateObjectLocalZ( 1, 100* f_delta )
    endif
    if GetRawKeyState(39) = 1
       RotateObjectLocalZ( 1, -100* f_delta )
    endif
    if GetRawKeyState(38) = 1
       MoveObjectLocalY(1, 3.0 * f_delta)
    endif
    if GetRawKeyState(40) = 1
       MoveObjectLocalY(1, -3.0 * f_delta)       
    endif
    
    
    SetVector3(fromVec3ID, GetObjectWorldX(1), GetObjectWorldY(1), GetObjectWorldZ(1))
    SetVector3(toVec3ID, GetObjectWorldX(2), GetObjectWorldY(2), GetObjectWorldZ(2))
    
    SetVector3(fromVec3ID1, GetObjectWorldX(3), GetObjectWorldY(3), GetObjectWorldZ(3))
    SetVector3(toVec3ID1, GetObjectWorldX(4), GetObjectWorldY(4), GetObjectWorldZ(4))
    
    RayCast3DPhysics( rayid, fromVec3ID, toVec3ID, 0 )
    RayCast3DPhysics( rayid1, fromVec3ID1, toVec3ID1, 0 )

    SetVector3(sfromVec3ID, GetObjectWorldX(3), GetObjectWorldY(3), GetObjectWorldZ(3))
    SetVector3(stoVec3ID, GetObjectWorldX(4), GetObjectWorldY(4), GetObjectWorldZ(4))

    SphereCast3DPhysics( srayid, sfromVec3ID, stoVec3ID, rad/80.0 ) //why? 80 idk

    sx = GetScreenXFrom3D( GetObjectWorldX(1), GetObjectWorldY(1), GetObjectWorldZ(1) ) 
    sy = GetScreenYFrom3D( GetObjectWorldX(1), GetObjectWorldY(1), GetObjectWorldZ(1) ) 
    sx1 = GetScreenXFrom3D( GetObjectWorldX(2), GetObjectWorldY(2), GetObjectWorldZ(2) ) 
    sy1 = GetScreenYFrom3D( GetObjectWorldX(2), GetObjectWorldY(2), GetObjectWorldZ(2) ) 
    
    DrawLine(sx,sy,sx1,sy1,col1,col2)

    sx = GetScreenXFrom3D( GetObjectWorldX(3), GetObjectWorldY(3), GetObjectWorldZ(3) ) 
    sy = GetScreenYFrom3D( GetObjectWorldX(3), GetObjectWorldY(3), GetObjectWorldZ(3) ) 
    sx1 = GetScreenXFrom3D( GetObjectWorldX(4), GetObjectWorldY(4), GetObjectWorldZ(4) ) 
    sy1 = GetScreenYFrom3D( GetObjectWorldX(4), GetObjectWorldY(4), GetObjectWorldZ(4) ) 
    
    DrawLine(sx,sy,sx1,sy1,col2,col1)

    sdist = (rad * 37.0)/2.0 // imperical
    for t = 0.0 to 1.0 step 0.25
    DrawEllipse((1-t)*sx + t*sx1 ,(1-t)*sy + t*sy1,sdist,sdist,col1,col2,0)
    next t
    
    print('sensor1:'+ str( Get3DPhysicsRayCastClosestObjectHit( rayid )))
    print('sensor2:'+ str( Get3DPhysicsRayCastClosestObjectHit( rayid1 )))

	print('spherecast closest object:'+ str( Get3DPhysicsRayCastClosestObjectHit( srayid )))
	
	Get3DPhysicsRayCastClosestContactPosition( srayid, soutVec3ID )
	
	sx = GetScreenXFrom3D( GetVector3X(soutVec3ID), GetVector3Y(soutVec3ID), GetVector3Z(soutVec3ID) ) 
	sy = GetScreenYFrom3D( GetVector3X(soutVec3ID), GetVector3Y(soutVec3ID), GetVector3Z(soutVec3ID) ) 
	
	DrawEllipse(sx, sy,sdist,sdist,col2,col2,1)

    
    h = 25
    sx = GetScreenXFrom3D( GetObjectWorldX(1), GetObjectWorldY(1), GetObjectWorldZ(1) ) - h
    sy = GetScreenYFrom3D( GetObjectWorldX(1), GetObjectWorldY(1), GetObjectWorldZ(1) ) - h
    sx1 = sx + 2*h
    sy1 = sy + 2*h
    DrawBox(sx,sy,sx1,sy1, col1, col1,col1,col1, 0 )

    h = 5
    sx = GetScreenXFrom3D( GetObjectWorldX(2), GetObjectWorldY(2), GetObjectWorldZ(2) ) - h
    sy = GetScreenYFrom3D( GetObjectWorldX(2), GetObjectWorldY(2), GetObjectWorldZ(2) ) - h
    sx1 = sx + 2*h
    sy1 = sy + 2*h
    DrawBox(sx,sy,sx1,sy1, col1, col1,col1,col1, 0 )

    h = 25
    sx = GetScreenXFrom3D( GetObjectWorldX(3), GetObjectWorldY(3), GetObjectWorldZ(3) ) - h
    sy = GetScreenYFrom3D( GetObjectWorldX(3), GetObjectWorldY(3), GetObjectWorldZ(3) ) - h
    sx1 = sx + 2*h
    sy1 = sy + 2*h
    DrawBox(sx,sy,sx1,sy1, col1, col1,col1,col1, 0 )

    h = 5
    sx = GetScreenXFrom3D( GetObjectWorldX(4), GetObjectWorldY(4), GetObjectWorldZ(4) ) - h
    sy = GetScreenYFrom3D( GetObjectWorldX(4), GetObjectWorldY(4), GetObjectWorldZ(4) ) - h
    sx1 = sx + 2*h
    sy1 = sy + 2*h
    DrawBox(sx,sy,sx1,sy1, col1, col1,col1,col1, 0 )

	
    
    if GetRawKeyState(27) = 1
    	end
    endif
    
    Step3DPhysicsWorld()
    Sync()
loop