.Newton Wrapper for Blitz3D

; ###############################################################################
; #					   JV-ODE - Dominator										#
; #					Code by Jim Williams (VIP3R) , Modified by wayneg			#
; #					Devious Codeworks - Copyright © 2006						#
; ###############################################################################

AppTitle "JV-ODE - Dominator  /  use mouse to push/pull"

Include "JV-ODE.bb"

Graphics3D 800,600,0,2
;Graphics3D 1440,900,2


Type ODEGeom
	Field body
	Field geom
	Field mesh
End Type

Global Toggle=0

; ################################################################################
; ### Setup ODE

Global World=dWorldCreate()
Global Space=dHashSpaceCreate(0)
Global ContactGroup=dJointGroupCreate(0)

dWorldSetAutoDisableFlag(World,1)
dWorldSetGravity(World,0,-0.98,0)
dContactSetMode(dContactBounce)
dContactSetBounce(0)
dContactSetMu(90)

; High Performance Settings
; Causes objects to settle down and sleep faster
dWorldSetAutoDisableLinearThreshold (World, .3)
dWorldSetAutoDisableAngularThreshold (World, .3)
dWorldSetAutoDisableSteps (World,16)
dContactSetMaxContacts(8)
dWorldSetQuickStepNumIterations (World,20) ; default 20

; ### Create light

Global Light=CreateLight()

RotateEntity Light,45,-90,0
LightColor Light,255,255,255
AmbientLight 130,130,130

; ### Create camera

Global Camera=CreateCamera()
CameraClsColor Camera,0,0,0
CameraRange Camera,1,1000
PositionEntity Camera,0,20,-50
RotateEntity Camera,30,0,0

; ### Create plane

dCreatePlane(Space,0,1,0,0)

Plane=CreatePlane()

EntityAlpha Plane,0.8

PlaneTexture=CreateTexture(128,128,9)

ClsColor 0,200,80
Cls

Color 255,255,255

Rect 0,0,64,64,1
Rect 64,64,64,64,1

CopyRect 0,0,128,128,0,0,BackBuffer(),TextureBuffer(PlaneTexture)
ScaleTexture PlaneTexture,20,20
EntityTexture Plane,PlaneTexture,0,0

Mirror=CreateMirror()

addobject()

; ###################################################################################################

While Not KeyHit(1)

; Calculate Time 
If T=0 Then T=MilliSecs()
T=MilliSecs() 

; Run Physics every 20ms ( APROX 50/sec )
If T > Time3 + 20 Then
	Time3=T	
	
	UpdateKeys()
	
	PTime=MilliSecs()
	UpdateGeoms()
	
	For i=1 To 2
		dSpaceCollide(Space,World,ContactGroup)
		dWorldQuickStep(World,.2)
		dJointGroupEmpty(ContactGroup)
	Next
	
	PhysicsTime#=MilliSecs()-PTime
End If

	RenderWorld

	Text 0,0,"JV-ODE Version "+dGetVersion()
	Text 0,15,"Physics Time:"+PhysicsTime

	Flip

Wend

dJointGroupDestroy(ContactGroup)
dSpaceDestroy(Space)
dWorldDestroy(World)
dCloseODE()

End

; ###################################################################################################

Function UpdateKeys()
If MouseDown(1)=1
	CameraPick(Camera,MouseX(),MouseY())
	If PickedEntity()
	
		For ode.ODEGeom=Each ODEGeom
			If ode\mesh=PickedEntity()
				dBodyEnable(ode\body)
				dBodyAddForce ode\body,-200*PickedNX#(),-200*PickedNY#(),-200*PickedNZ#()
				End If
		Next
		End If
	End If

If MouseDown(2)=1
	CameraPick(Camera,MouseX(),MouseY())
	If PickedEntity()
		For ode.ODEGeom=Each ODEGeom
			If ode\mesh=PickedEntity()
				dBodyEnable(ode\body)
				dBodyAddForce ode\body,200*PickedNX#(),200*PickedNY#(),200*PickedNZ#()
				End If
		Next
		End If
	End If

End Function

; ###################################################################################################

Function AddObject()

For i=1 To 200
	radius# = 10+i/10.0
	ugol# = ugol# + 120.0/radius
	
	xp# = radius*Sin(ugol)
	zp# = radius*Cos(ugol)
	
	ode.ODEGeom=New ODEGeom
	ode\body=dBodyCreate(World)
	
	dBodySetRotation(ode\body,0,120-ugol,0)
	dBodySetPosition(ode\body,xp,1.6,zp)

	dBodySetAutoDisableFlag(ode\body,1)
	ode\geom=dCreateBox(Space,1.5,3,.7)
	
	dGeomSetBody(ode\geom,ode\body)
	ode\mesh=CreateCube()
	
	ScaleMesh ode\mesh,1.5*.5,3*.5,.7*.5
	
	EntityColor ode\mesh,Rand(255),Rand(255),Rand(255)
	EntityAlpha ode\mesh,1
	EntityShininess ode\mesh,0.7
	
	EntityPickMode ode\mesh,2
	
	mass=dMassCreate()
	dMassSetBoxTotal(mass,300,1.5,3,.7)
	dBodySetMass(ode\body,mass)
	dMassDestroy(mass)
Next

End Function

; ###################################################################################################

Function UpdateGeoms()

For ode.ODEGeom=Each ODEGeom
	If dBodyIsEnabled(ode\body)
		RotateEntity ode\mesh,dGeomGetPitch#(ode\geom),dGeomGetYaw#(ode\geom),dGeomGetRoll#(ode\geom)
		PositionEntity ode\mesh,dGeomGetPositionX#(ode\geom),dGeomGetPositionY#(ode\geom),dGeomGetPositionZ#(ode\geom)
	Else
		dGeomContactSetMu(ode\geom,20)
	End If
Next

End Function

; ###################################################################################################

typedef void(WINAPI *DLL_phWorldCreate)(int plane, const char *license_key);

Include "newton.bb"

; Author : patmaba
; email : patmaba@...
; date : 2010 Jul 19

Const k_APP_NAME$ = "Blitz3d And Newton Physic Engine : Example Follow Waypoint Smoothly"
Const K_SCRWIDTH=800 
Const K_SCRHEIGHT=600
Const K_MAX_WAYPOINT = 6

Global g_TCAR_TEMPLATE	=	0
Global g_Cam    =   0
Global g_TCar_Player.TCar = Null 
Global g_bWireFrame = False


Dim gd_wp(K_MAX_WAYPOINT) ; waitpoint


App_Init()
App_Main()
App_End()
End 

; main loop

Function App_Main()
	
	While Not(KeyHit(1))
		
		; update physics by 0.01 second
		phWorldStep(0.01)
		
		TCar_Update()
		
		
		RenderWorld
		
		Color 255,255,255
	    Text 1,1, "WireFrame(  'W' )   
		
		RenderContacts( g_TCar_Player\body , 255,0,0,g_Cam)
		
		Color 255,255,255
		For i=0 To K_MAX_WAYPOINT-1
			If EntityInView( gd_wp(i), g_Cam )
				CameraProject g_Cam,EntityX(gd_wp(i)),EntityY(gd_wp(i)),EntityZ(gd_wp(i))
				Text ProjectedX()-22, ProjectedY()+10, EntityName(gd_wp(i))		
			EndIf  
		Next 
		
		
		Color 255,255,255
		CameraProject g_Cam,EntityX(g_TCar_Player\entity),EntityY(g_TCar_Player\entity),EntityZ(g_TCar_Player\entity)
		Text ProjectedX()-25, ProjectedY()+25, "Player"		
		
		If KeyHit(44) Or KeyHit(17) Then 
			g_bWireFrame = 1-g_bWireFrame
			WireFrame g_bWireFrame 
		EndIf 
		
		Flip
	Wend
	
End Function 


Function App_End()
	TCar_DeleteAll()
	ClearWorld True,True,True
	EndGraphics
End Function 



Function App_Init()
	; Initialize Graphics And window title
	Graphics3D K_SCRWIDTH,K_SCRHEIGHT,16,2
	AppTitle k_APP_NAME$
	SeedRnd MilliSecs()
 	; Create camera and position it
	Local cam = CreateCamera()
	PositionEntity cam,0,3,-10
	
    ; Initialize physics
        ;<NWT>
	phWorldCreate(True,"your license key") ;<NWT> ; if parameter is set to True, a horizontal plane at (0,0,0) will be created
	phWorldSetGravity(0,-40,0) ;<NWT>
	
 	; Create plane and texturize it
	Local plane = CreatePlane()
	EntityColor plane,50,200,255
	
	; Create texture of size 256x256 
	Local tex=CreateTexture( 256,256 ) 
	
	; Set buffer - texture buffer 
	SetBuffer TextureBuffer( tex )
	
	; Clear texture buffer with background white color 
	ClsColor 255,255,0 
	Cls 
	Color 0,0,0
	Rect 10,10,236,236
	EntityTexture plane,tex
	FreeTexture tex
	
	SetBuffer FrontBuffer() 
	
	 ; Create light and rotate it
	Local light = CreateLight(1)
	RotateEntity light,20,30,40
	AmbientLight 255,255,255
	
	g_TCAR_TEMPLATE = TCar_MeshTemplate()
	
	g_TCar_Player = TCar_Create( 0,0,-15, 0, 0, 0 )
	
	EntityColor g_TCar_Player\entity,255,255,250
	phBodySetMass( g_TCar_Player\body, 1000 );<NWT>				
	
	
	
	For i=0 To K_MAX_WAYPOINT-1
		gd_wp(i) = CreateSphere(12) 
		ScaleEntity gd_wp(i), 2,2,2
		EntityColor gd_wp(i), 255,0,0
		EntityAlpha gd_wp(i),0.5
		NameEntity gd_wp(i), "WayPoint " + i
	Next 
	
	PositionEntity gd_wp(0),  0,1,0
	PositionEntity gd_wp(1),-10,1,15
	PositionEntity gd_wp(2), 0,1,30
	PositionEntity gd_wp(3), 30,1,0
	PositionEntity gd_wp(4), 15,1,45
	PositionEntity gd_wp(5), 30,1,30
	
	
	; Buil 50 random wall
	Local c.phx=Null
	Local x#,y#,z#,yaw#,n
	For n=0 To 100
		c = phxCreateBox( 1,1,1,10)
		x#=Rand(-15,45) : y#=10 : z#=Rand(-15,45)
		yaw#=Rnd(0,360)
		PositionEntity (c\mesh,x#, y#, z#)
		RotateEntity (c\mesh,yaw#, yaw#, yaw#)
		EntityColor( c\mesh, Rand(0,255),Rand(0,255),Rand(0,255) )
		ScaleMesh c\mesh, Rand(1,2), Rand(1,2), Rand(1,2)
		
		phBodySetEntity(c\body,c\mesh);<NWT>
		phBodySetPos(c\body,x#, y#, z#);<NWT>		; body,x#,y#,z#
		phBodySetRot(c\body,0,yaw#,0);<NWT>		; body,pitch#,yaw#,roll#
		phBodySetMass( c\body, 100 );<NWT>
	Next 
	
	
	g_Cam = cam
End Function 




Type TCar
	Field entity
	Field body
	Field CurrentWpIndex
End Type 



Function TCar_MeshTemplate()
	; Build a car Template
	Local l_MDL	=	CreateCube()
	HideEntity(l_MDL) 
	ScaleMesh		(l_MDL,1,.35,2) 
	PositionMesh	(l_MDL,0,.5,0) 
	Local cabine%=CreateCube() 
	ScaleMesh		(cabine,.9,.5,1) 
	PositionMesh	(cabine,0,1,-.3) 
	AddMesh			(cabine,l_MDL) 
	FreeEntity		(cabine) 
	Return l_MDL
End Function 


Function TCar_Create.TCar( pX#=0, pY#=0, pZ#=0, pPitch#=0,pYaw#=0,pRoll#=0 )
	Local o.TCar = New TCar
	
	o\entity = CopyEntity(g_TCAR_TEMPLATE)
	o\body = BodyCreateHull(o\entity,1);<NWT>
	o\CurrentWpIndex=0
    ; Set the position of the body
	phBodySetPos(o\body,pX#, pY#, pZ#)		; body,x#,y#,z#
	
	 ; Set the rotation of the body
	phBodySetRot(o\body,pPitch#,pYaw#,pRoll#);<NWT>		; body,pitch#,yaw#,roll#
	
	EntityColor o\entity, Rnd(0,255), Rnd(0,255), Rnd(0,255)
	
	
	phBodySetMass( o\body, 10000 );<NWT>
	
	
	Return o
End Function 


Function TCar_DeleteAll()
	Local c.TCar = Null
	For c.TCar = Each TCar
		TCar_Delete( c )
	Next 
End Function  


Function TCar_Delete( c.TCar )
	phBodyDestroy(c\body)
	FreeEntity c\entity
	Delete c
End Function 




Function TCar_Position( c.TCar)
	
    ; Set the position of the body
	PositionEntity c\entity,phBodyGetX(c\body),phBodyGetY(c\body),phBodyGetZ(c\body)
End Function 



Function TCar_Rotate( c.TCar)
	
	
	 ; Set the rotation of the body
	RotateEntity c\entity, phBodyGetPitch(c\body),phBodyGetYaw(c\body),phBodyGetRoll(c\body)
End Function 






Function TCar_Update()
	
	TCar_FollowWayPoint( g_TCar_Player )
	
	Cam_3rd_Pers( g_TCar_Player )
	
	
End Function 



Function SmoothPoint(source%,dest%,rate#=1) 
	Dx#=EntityX(dest,1)-EntityX(source,1) 
	Dy#=EntityY(dest,1)-EntityY(source,1) 
	Dz#=EntityZ(dest,1)-EntityZ(source,1) 
	AlignToVector source,dx#,dy#,dz#,3,rate 
End Function 



Function TCar_FollowWayPoint( c.TCar )
	If EntityDistance( gd_wp( c\CurrentWpIndex), c\entity ) < 2 Then
		c\CurrentWpIndex = c\CurrentWpIndex + 1;
		If c\CurrentWpIndex > K_MAX_WAYPOINT-1 Then c\CurrentWpIndex = 0
	EndIf 
	
	TCar_SmoothMoveToWayPoint( c.TCar,0,0,15,0.03 )
	
	; Apply Physics Position and rotation To Blitz Entity
    TCar_Position( c)
    TCar_Rotate( c )
	
End Function 		  



Function TCar_SmoothMoveToWayPoint( c.TCar, pf_Mvx#=0.0, pf_Mvy#=0.0, pf_Mvz#=0.0, pf_Rate#=1.0 )
	
    SmoothPoint(c\entity, gd_wp( c\CurrentWpIndex),pf_Rate#) 
	;PointEntity c\entity, gd_wp( c\CurrentWpIndex)
	
	Local oldx#=EntityX(c\entity)
	Local oldy#=EntityY(c\entity)
	Local oldz#=EntityZ(c\entity)
	
	MoveEntity c\entity, pf_Mvx#, pf_Mvy#, pf_Mvz#
	
	Local newx#=EntityX(c\entity)
	Local newy#=EntityY(c\entity)
	Local newz#=EntityZ(c\entity)
	
	Local vx#=newx#-oldx#
	Local vy#=newy#-oldy#
	Local vz#=newz#-oldz#
	
	phBodySetVel( c\body, vx# , vy#, vz# )
	
	Local p=0
	Local yw=EntityYaw(g_TCar_Player\entity,True)
	Local r=0
	
	phBodySetRot(g_TCar_Player\body,p, yw, r )
	
End Function 

Function Cam_3rd_Pers( c.TCar ) 
	TFormPoint 0,4,-10,c\entity,0 
	Local dx#, dy#, dz# 
	dx#=(TFormedX()-EntityX(g_Cam))*.1 
	dy#=(TFormedY()-EntityY(g_Cam))*.1 
	dz#=(TFormedZ()-EntityZ(g_Cam))*.1 
	
	TranslateEntity g_Cam,dx,dy,dz 
	
	PointEntity g_Cam,c\entity 
End Function 	



Function RenderContacts(body,r%,g%,b%,cam)
	Local ncoll = phBodyGetCollNum(body)
	Local i,x#,y#,z#,sx#,sy#,sx2#,sy2#
	For i = 0 To ncoll-1
		Color r,g,b
		x# = phBodyGetCollX(body,i)
		y# = phBodyGetCollY(body,i)
		z# = phBodyGetCollZ(body,i)
		If y# > 0.0
			CameraProject cam,x,y,z
			sx# =ProjectedX():sy# =ProjectedY()
			Oval sx-2,sy-2,4,4
			x# = x# + phBodyGetCollNX(body,i)
			y# = y# + phBodyGetCollNY(body,i)
			z# = z# + phBodyGetCollNZ(body,i)
			CameraProject cam,x,y,z
			sx2# =ProjectedX():sy2# =ProjectedY()
			Line sx,sy,sx2,sy2
			Color 255,255,255
		EndIf 
	Next
End Function

;================================================================================================
;Name:  phSetMeshByBody( p_iMesh, p_iBody, p_bGlobal )
;Parameters :
;p_iMesh - Handle of Blitz3d mesh to be positionned and rotated
;p_iBody - Handle of Newton Physic Engine use for applying position and rotation on Blitz3d Mesh
;p_bGlobal (optional) - True If the position should be relative To 0,0,0 rather 
;                       than a parent entity's position. False by Default.  
;		
;Description
;		Apply newton physics body positions and rotation on Blitz3d mesh.
;
;version   date          who             what
;-------   ----------    -------------   --------------------------------------------------------
;    1.0   2010/07/29    patmaba         creation 
;================================================================================================
Function phSetMeshByBody( p_iMesh, p_iBody, p_bGlobal=False )
	; Set mesh position 
    Local l_fx#=phBodyGetX(p_iBody)
	Local l_fy#=phBodyGetY(p_iBody)
	Local l_fz#=phBodyGetZ(p_iBody)
	PositionEntity p_iMesh, l_fx#, l_fy#, l_fz#, p_bGlobal
	
	; Set mesh rotation 
    Local l_fPitch#=phBodyGetPitch(p_iBody)
	Local l_fYaw#=phBodyGetYaw(p_iBody)
	Local l_fRoll#=phBodyGetRoll(p_iBody)
	
	RotateEntity p_iMesh, l_fPitch#, l_fYaw#, l_fRoll#, p_bGlobal
End Function 

;================================================================================================
;Name:  phSetBodyByMesh( p_iBody, p_iMesh, p_bGlobal )
;
;Parameters :
;p_iBody - Handle of Newton Physic Engine use for applying position and rotation on Blitz3d Mesh
;p_iMesh - Handle of Blitz3d mesh to be positionned and rotated
;p_bGlobal (optional) - True For Global coordinates, False For Local. Optional, defaults To False.  
;		
;Description
;		Apply Blitz3d mesh positions and rotation on newton physics body.
;		
;		If the Global flag is set To False Then the parent's Local coordinate system is used. 
;		NOTE: If the entity has no parent Then Local and Global coordinates are the same. 
;	          In this Case you can think of the 3d world as the parent.  
;
;version   date          who             what
;-------   ----------    -------------   --------------------------------------------------------
;    1.0   2010/07/29    patmaba         creation 
;================================================================================================
Function phSetBodyByMesh( p_iBody, p_iMesh, p_bGlobal=True )
    ; Set body position
    Local l_fx#=EntityX(p_iMesh,p_bGlobal)
	Local l_fy#=EntityY(p_iMesh,p_bGlobal)
	Local l_fz#=EntityZ(p_iMesh,p_bGlobal)
	phBodySetPos(p_iBody, l_fx#, l_fy#, l_fz#)
	
	 ; Set body rotation
    Local l_fPitch#=EntityPitch(p_iMesh,p_bGlobal)
	Local l_fYaw#=EntityYaw(p_iMesh,p_bGlobal)
	Local l_fRoll#=EntityRoll(p_iMesh,p_bGlobal)
	phBodySetRot(p_iBody, l_fPitch#, l_fYaw#, l_fRoll#)
End Function