
SuperStrict

Import newton.dynamics
Import openb3d.b3dglgraphics

Type TNewtonBody Extends TNBody

	Field mesh:TMesh
	Field mass:Float
	Field shadow:TShadowObject
	
	Method OnTransform(matrix:Float Ptr, threadIndex:Int)
		AlignEntity matrix
	End Method

	Method OnForceAndTorque(timestamp:Float, threadIndex:Int)

		Local _mass:Float
	 	Local Ixx:Float, Iyy:Float, Izz:Float
		
		GetMassMatrix _mass, Ixx, Iyy, Izz
		
		' Gravity only...
		
		SetForce 0.0 * _mass, -0.987 * _mass, 0.0 * _mass, 1.0

	EndMethod

	Method ToggleShadow (toggle:Int)
	
		If toggle
			shadow = CreateShadow (mesh)
		Else
			If shadow
				FreeShadow shadow
				shadow = Null
			EndIf
		EndIf
	
	End Method
	
	Method Remove ()

		ToggleShadow 0
		
		If mesh
			FreeEntity mesh
			mesh = Null
		EndIf
		
		Self.Destroy
		
	End Method

	' Fill in Newton TNMatrix from OpenB3D's TEntity.mat[:TMatrix].grid...
	
	Method AlignNewtonMatrix:Int (newton_matrix:TNMatrix)
		MemCopy newton_matrix, mesh.mat.grid, 64
	End Method
	
	' Fill in OpenB3D's TEntity.mat[:TMatrix].grid from Newton TNMatrix...
	
	Method AlignEntity:Int (newton_matrix:Float Ptr)

		mesh.MQ_ApplyNewtonTransform newton_matrix


		' ALL BELOW IS OBSOLETE... I think...
		
		
				
		
		' ---------------------------------------------------------------------------
		' Receive NEWTON matrix:
		' ---------------------------------------------------------------------------
		
		'Local temp:TMatrix = TMatrix.CreateObject (TEntity.GetInstance (Self.mesh))
		
		'	temp.grid = newton_matrix

		' Print NEWTON matrix:
		
		'PrintMatrix temp, 1
		
		
		
		' ---------------------------------------------------------------------------
		' Update B3D matrix from NEWTON matrix:
		' ---------------------------------------------------------------------------
		
		
	
		'mesh.MQ_SetMatrix temp



		' ---------------------------------------------------------------------------
		' Copy NEWTON matrix over B3D matrix:
		' ---------------------------------------------------------------------------

		' This SHOULD be done by "mesh.MQ_SetMatrix temp"!
		
		'MemCopy mesh.mat.grid, newton_matrix, 64



		' Print B3D matrix:
		
		'PrintMatrix mesh.mat, 2
		
		
		



	End Method

End Type

' Just for visual comparison with C++!

Function PrintMatrix:Int (matrix:TMatrix, indent:Int = 0)
	
	Local id:String
	
	For Local loop:Int = 0 Until indent
		id = id + "~t"
	Next
	
	Print id + "B3D:"
	Print ""
	
	For Local y:Float = 0 To 3
		Print id + matrix.grid[y * 4] + "~t" + matrix.grid[y * 4 + 1] + "~t" + matrix.grid[y * 4 + 2] + "~t" + matrix.grid[y * 4 + 3] + "~t"
	Next

	Print ""
	
End Function

