Strict
Framework PUB.Newton
Import GG.IrrBMax
Import BRL.PNGLoader
Local device:T_irrIrrlichtDevice = T_irrIrrlichtDevice.create(EDT_OPENGL , T_irrDimension2d_s32.create(640 , 480) , 16 , False , False , False , 0)
Global driver:T_irrIVideoDriver = device.getVideoDriver()
Global smgr:T_IrrISceneManager = device.getSceneManager() 
'Local Tex:T_irrITexture = driver.getTexture("brlLogo.png")
Local camera:T_irrICameraSceneNode = smgr.addCameraSceneNodeFPS()
camera.setPosition(T_irrVector3df.createFromVals(-19,-49,-30))
camera.setRotation(T_irrVector3df.createFromVals(0 , 0 , 0))
Local light:T_irrILightSceneNode = smgr.addLightSceneNode( 0 , T_irrVector3df.createfromVals(-20 , -40 , -30) , 100)
Global nWorld:Byte Ptr = newtonCreate(PhysicsAlloc , PhysicsFree)
initScene()
Local Timer:Int = MilliSecs()
While (device.run())
    Timer:+MilliSecs()
    newtonUpdate(nWorld , 0.001)
    Timer = MilliSecs()
    'NewtonBodyGetMatrix(box , Varptr Mat[0])
    'T_irrSetMatrix(node , Mat)
    driver.beginScene(True , True , T_irrSColor.createFromVals(0,200 , 200 , 200))
    smgr.drawAll()
    
    driver.endScene()
    FlushMem
Wend
Rem '''''''''''''''''''''''''''''''''''/////////////////////////////////////   The Convertion Matrix Functions
        T_irrGetMatrix is the function to get the matrix from the Irrlecht SceneNode And pass it
    to an array to be used by Newton.
        T_irrSetMatrix is the function to pass the Matrix from Newton to the Irrlicht SceneNode
EndRem
Function T_irrGetMatrix(NSceneNode:T_irrISceneNode , NewtonMat:Float Ptr)
    Local Euler:Float[3]
    Euler[0] = NSceneNode.getRotation().getX() * (Pi / 180.0)
    Euler[1] = NSceneNode.getRotation().getY() * (Pi / 180.0)
    Euler[2] = NSceneNode.getRotation().getZ() * (Pi / 180.0)
    NewtonSetEulerAngle(Varptr Euler[0] , Varptr newtonMat[0])
    Newtonmat[12] = NSceneNode.getPosition().getX()
    newtonMat[13] = NSceneNode.getPosition().getY()
    NewtonMat[14] = NSceneNode.getPosition().getZ()    
End Function
Function T_irrSetMatrix(NSceneNode:T_irrISceneNode , NewtonMat:Float Ptr)
    Local Euler:Float[3]
    NewtonGetEulerAngle(Varptr NewtonMat[0] , Varptr Euler[0])
    NSceneNode.setPosition(T_irrVector3df.createFromVals(NewtonMat[12] , NewtonMat[13] , NewtonMat[14]))
    NSceneNode.setRotation(T_irrVector3df.createfromVals(Euler[0] * (180 / Pi) , Euler[1] * (180 / Pi) , Euler[2] * (180 / Pi)))
End Function
Rem 
                    THE FUNCTIONS FROM TUTORIAL 2
End Rem
Function    PhysicsAlloc:Byte Ptr(sizeInBytes:Int)
    Return MemAlloc(sizeInBytes)
End Function
Function    PhysicsFree( memptr:Byte Ptr, sizeInBytes:Int)
    MemFree (memptr, sizeInBytes)
End Function
' set the tranformation of a rigid body
Function  PhysicsApplyForceAndTorque (Body:Byte Ptr)
    Local mass:Float
    Local Ixx:Float
    Local Iyy:Float
    Local Izz:Float
    NewtonBodyGetMassMatrix (body,Varptr mass, Varptr Ixx, Varptr Iyy, Varptr Izz);
    'Local force:Float[] =  [0.0, -mass * 9.8 , 0.0]
    ' GG: messed with the mass multiplier to make it look like the tutorial.  have no idea why this is different.
    Local force:Float[] =  [0.0, -mass * 410 , 0.0]
    NewtonBodySetForce (body, Varptr force[0])
End Function
Function  PhysicsSetTransform (body:Byte Ptr, NewtonMat:Float Ptr)
    Local primitive:T_irrISceneNode
    '// get the graphic Object form the rigid body
    ' GG: changed to recreate the scenenode based on the C++ object handle
    primitive = T_irrISceneNode.CreateFromHandle(Int(newtonBodyGetUserData(body)),False)
    
    '// set the transformation matrix For this rigid body
    newtonBodyGetMatrix(body , Varptr NewtonMat[0])
    T_irrSetMatrix(Primitive , Varptr NewtonMat[0])
    
End Function
Function PhysicsBodyDestructor(body:Byte Ptr)
    Local primitive:T_irrISceneNode
    '// get the graphic Object form the rigid body
    ' GG: changed to recreate the scenenode based on the C++ object handle
    primitive = T_irrISceneNode.CreateFromHandle(Int(newtonBodyGetUserData(body)),False)
    '// destroy the graphic Object
    primitive.remove()
    
End Function
Function CleanUp ()
    '// destroy the Newton world
    NewtonDestroy (nWorld);
    
End Function
Function InitScene()
    Global Texture:T_irrITexture = Driver.getTexture("doc/brllogo.png") 
    Global box:T_irrISceneNode
    Global IFloor:T_irrISceneNode
    Global boxBody:Byte Ptr
    Global floorBody:Byte Ptr
    Global collision:Byte Ptr
    Global Mat:Float[16]
    '// create the newton world
    
    '// set the linear solver model For faster speed 
    NewtonSetSolverModel(nWorld, 8)
    '// set the adpative friction model For faster speed 
    NewtonSetFrictionModel(nWorld, 1)
    
    '// Set the termination Function
    'atexit(CleanUp);   '//// I Haven't Found The Apropriaty Function for Irrlicht
    
    '// create the the Floor graphic objects
    IFloor = smgr.addTestSceneNode(100 )
    'IFloor.setMaterialFlag(EMF_LIGHTING,True)
    IFloor.setMaterialTexture(0 , texture)
    IFloor.setPosition(T_irrVector3df.createFromVals(0,-100 , 0))
    collision = newtonCreateBox(nWorld , 100.0 , 100.0 , 100.0 , Null)
    FloorBody = newtonCreateBody(nWorld , collision)
    
    '// set the transformation For this rigid body
    T_irrGetMatrix(IFloor ,Varptr Mat[0])
    NewtonBodySetMatrix(floorBody,Varptr Mat[0])
    
    '// save the pointer To the graphic Object with the body.
    ' GG: changed to store the scenenode C++ object handle instead of a reference to the BMAX object with falls out of scope
    NewtonBodySetUserData(floorBody,Byte Ptr(IFloor.handle))
    
    '// set a destrutor For this rigid body
    NewtonBodySetDestructorCallback(floorBody, PhysicsBodyDestructor);
    
    '// set the initial size
    'size = dVector(0.5f, 0.5f, 0.5f);    Not really nesesery(I think so)
    '// create the collision 
    newtonReleaseCollision(nWorld , collision)
    collision = NewtonCreateBox(nWorld, 0.5 , 0.5, 0.5 , Null) 
    Local location:Float[] = [0.0 , 0.0 , 0.0]
    location[0] = -10.0   'FOR  X
    For Local k:Int = 0 To 9
        location[2] = 0.0    'For Z
        For Local J:Int = 0 To 9
            location[1] = 2.0    'For Y
            For Local i:Int = 0  To 9
                box = smgr.addTestSceneNode(0.5)
                box.setMaterialFlag(EMF_LIGHTING,True)
                box.SetMaterialTexture(0 , Texture)
                box.setPosition(T_irrVector3df.createFromVals(location[0] ,location[1] , location[2]))
                boxBody = newtonCreateBody(nWorld , collision)                
                ' GG: changed to store the scenenode C++ object handle instead of a reference to the BMAX object with falls out of scope
                newtonBodySetUserData(boxBody ,Byte Ptr(box.handle))        
                '// set a destrutor For this rigid body
                NewtonBodySetDestructorCallback(boxBody, PhysicsBodyDestructor)
                '// set the tranform call back Function
                NewtonBodySetTransformCallback(boxBody, PhysicsSetTransform)
                '// set the force And torque call back funtion
                NewtonBodySetForceAndTorqueCallback(boxBody, PhysicsApplyForceAndTorque)
                '// set the mass matrix
                '//NewtonBodySetMassMatrix (boxBody, 1.0f, 1.0f / 6.0f, 1.0f / 6.0f, 1.0f  / 6.0f);
                NewtonBodySetMassMatrix(boxBody, 1.0, 1.0, 1.0, 1.0)                
                '// set the matrix For tboth the rigid nody And the graphic body
                T_irrGetMatrix(box , Varptr Mat[0])
                NewtonBodySetMatrix(boxBody,Varptr Mat[0])
                PhysicsSetTransform(boxBody, Varptr Mat[0])
                Location[1] :+ 0.5 * 2.0
            Next
            Location[2] :- 0.5 * 4.0     
        Next
        location[0] :- 0.5 * 4.0 
    Next
    '// Release the collsion geometry when Not need it
    NewtonReleaseCollision(nWorld, collision)
EndFunction