module Physics.Bullet.Raw.Utils where

import Control.Monad as M

import Physics.Bullet.Raw
import Physics.Bullet.Raw.Types
import Physics.Bullet.Raw.Class

import Data.Vect.Float

idTransform :: Transform
idTransform :: Transform
idTransform = Mat3 -> Vec3 -> Transform
Transform Mat3
forall m. Matrix m => m
idmtx Vec3
forall g. AbelianGroup g => g
zero

simpleBtDiscreteDynamicsWorldM :: IO BtDiscreteDynamicsWorld
simpleBtDiscreteDynamicsWorldM :: IO BtDiscreteDynamicsWorld
simpleBtDiscreteDynamicsWorldM = do
    BtDefaultCollisionConstructionInfo
dc <- IO BtDefaultCollisionConstructionInfo
btDefaultCollisionConstructionInfo
    BtDefaultCollisionConfiguration
c <- BtDefaultCollisionConstructionInfo
-> IO BtDefaultCollisionConfiguration
forall p0.
BtDefaultCollisionConstructionInfoClass p0 =>
p0 -> IO BtDefaultCollisionConfiguration
btDefaultCollisionConfiguration BtDefaultCollisionConstructionInfo
dc
    
    BtCollisionDispatcher
d <- BtDefaultCollisionConfiguration -> IO BtCollisionDispatcher
forall p0.
BtCollisionConfigurationClass p0 =>
p0 -> IO BtCollisionDispatcher
btCollisionDispatcher BtDefaultCollisionConfiguration
c
    BtHashedOverlappingPairCache
hc <- IO BtHashedOverlappingPairCache
btHashedOverlappingPairCache
    BtDbvtBroadphase
b <- BtHashedOverlappingPairCache -> IO BtDbvtBroadphase
forall p0.
BtOverlappingPairCacheClass p0 =>
p0 -> IO BtDbvtBroadphase
btDbvtBroadphase BtHashedOverlappingPairCache
hc
    BtSequentialImpulseConstraintSolver
s <- IO BtSequentialImpulseConstraintSolver
btSequentialImpulseConstraintSolver
    BtCollisionDispatcher
-> BtDbvtBroadphase
-> BtSequentialImpulseConstraintSolver
-> BtDefaultCollisionConfiguration
-> IO BtDiscreteDynamicsWorld
forall p0 p1 p2 p3.
(BtDispatcherClass p0, BtBroadphaseInterfaceClass p1,
 BtConstraintSolverClass p2, BtCollisionConfigurationClass p3) =>
p0 -> p1 -> p2 -> p3 -> IO BtDiscreteDynamicsWorld
btDiscreteDynamicsWorld BtCollisionDispatcher
d BtDbvtBroadphase
b BtSequentialImpulseConstraintSolver
s BtDefaultCollisionConfiguration
c
{-
simpleBtContinuousDynamicsWorldM :: IO BtContinuousDynamicsWorld
simpleBtContinuousDynamicsWorldM = do
    dc <- btDefaultCollisionConstructionInfo
    c <- btDefaultCollisionConfiguration dc
    
    d <- btCollisionDispatcher c
    hc <- btHashedOverlappingPairCache
    b <- btDbvtBroadphase hc
    s <- btSequentialImpulseConstraintSolver
    btContinuousDynamicsWorld d b s c
-}

localCreateRigidBodyM :: (BtDynamicsWorldClass bc, BtCollisionShapeClass p2) => bc -> Float -> Transform -> p2 -> IO (BtDefaultMotionState, BtRigidBody)
localCreateRigidBodyM :: bc
-> Float
-> Transform
-> p2
-> IO (BtDefaultMotionState, BtRigidBody)
localCreateRigidBodyM dw :: bc
dw mass :: Float
mass startTransform :: Transform
startTransform shape :: p2
shape = do
    Vec3
inertia <- case Float
mass Float -> Float -> Bool
forall a. Eq a => a -> a -> Bool
/= 0 of
        True    -> p2 -> Float -> Vec3 -> IO Vec3
forall bc.
BtCollisionShapeClass bc =>
bc -> Float -> Vec3 -> IO Vec3
btCollisionShape_calculateLocalInertia p2
shape Float
mass Vec3
forall g. AbelianGroup g => g
zero
        False   -> Vec3 -> IO Vec3
forall (m :: * -> *) a. Monad m => a -> m a
return Vec3
forall g. AbelianGroup g => g
zero
    
    BtDefaultMotionState
motionState <- Transform -> Transform -> IO BtDefaultMotionState
btDefaultMotionState Transform
startTransform Transform
idTransform
    BtRigidBody
body <- Float -> BtDefaultMotionState -> p2 -> Vec3 -> IO BtRigidBody
forall p1 p2.
(BtMotionStateClass p1, BtCollisionShapeClass p2) =>
Float -> p1 -> p2 -> Vec3 -> IO BtRigidBody
btRigidBody1 Float
mass BtDefaultMotionState
motionState p2
shape Vec3
inertia
    BtRigidBody -> Float -> IO ()
forall bc. BtCollisionObjectClass bc => bc -> Float -> IO ()
btCollisionObject_setContactProcessingThreshold BtRigidBody
body (1e30)
    bc -> BtRigidBody -> IO ()
forall bc p0.
(BtDynamicsWorldClass bc, BtRigidBodyClass p0) =>
bc -> p0 -> IO ()
btDynamicsWorld_addRigidBody bc
dw BtRigidBody
body
    (BtDefaultMotionState, BtRigidBody)
-> IO (BtDefaultMotionState, BtRigidBody)
forall (m :: * -> *) a. Monad m => a -> m a
return (BtDefaultMotionState
motionState,BtRigidBody
body)

mkVehicleM :: (BtDynamicsWorldClass bc,  BtCollisionShapeClass p1) => bc -> p1 -> Float -> IO (BtDefaultMotionState, BtRigidBody, BtRaycastVehicle)
mkVehicleM :: bc
-> p1
-> Float
-> IO (BtDefaultMotionState, BtRigidBody, BtRaycastVehicle)
mkVehicleM dw :: bc
dw chassisShape :: p1
chassisShape mass :: Float
mass = do
    --chassisShape <- btBoxShape $ Vector3 1 0.6 2.2
    BtCompoundShape
compound <- Bool -> IO BtCompoundShape
btCompoundShape Bool
True
    let localTrans :: Transform
localTrans = Mat3 -> Vec3 -> Transform
Transform Mat3
forall m. Matrix m => m
idmtx Vec3
forall g. AbelianGroup g => g
zero
        wheelRadius :: Float
wheelRadius = 0.5
        wheelWidth :: Float
wheelWidth = 0.4
    BtCompoundShape -> Transform -> p1 -> IO Transform
forall bc p1.
(BtCompoundShapeClass bc, BtCollisionShapeClass p1) =>
bc -> Transform -> p1 -> IO Transform
btCompoundShape_addChildShape BtCompoundShape
compound Transform
localTrans p1
chassisShape
    
    (carMotionSate :: BtDefaultMotionState
carMotionSate,carChassis :: BtRigidBody
carChassis) <- bc
-> Float
-> Transform
-> BtCompoundShape
-> IO (BtDefaultMotionState, BtRigidBody)
forall bc p2.
(BtDynamicsWorldClass bc, BtCollisionShapeClass p2) =>
bc
-> Float
-> Transform
-> p2
-> IO (BtDefaultMotionState, BtRigidBody)
localCreateRigidBodyM bc
dw Float
mass Transform
idTransform BtCompoundShape
compound
    --(carMotionSate,carChassis) <- localCreateRigidBody dw 8 (Transform idMatrix3x3 $ Vector3 480.0 20.3 (-520.0)) compound
    --wheelShape <- btCylinderShapeX $ Vector3 wheelWidth wheelRadius wheelRadius
    BtRigidBody -> Transform -> IO Transform
forall bc. BtRigidBodyClass bc => bc -> Transform -> IO Transform
btRigidBody_setCenterOfMassTransform BtRigidBody
carChassis Transform
idTransform
    BtRigidBody -> Vec3 -> IO Vec3
forall bc. BtRigidBodyClass bc => bc -> Vec3 -> IO Vec3
btRigidBody_setLinearVelocity BtRigidBody
carChassis Vec3
forall g. AbelianGroup g => g
zero
    BtRigidBody -> Vec3 -> IO Vec3
forall bc. BtRigidBodyClass bc => bc -> Vec3 -> IO Vec3
btRigidBody_setAngularVelocity BtRigidBody
carChassis Vec3
forall g. AbelianGroup g => g
zero

    BtRaycastVehicle_btVehicleTuning
tuning <- IO BtRaycastVehicle_btVehicleTuning
btRaycastVehicle_btVehicleTuning    
    BtDefaultVehicleRaycaster
vehicleRayCaster <- bc -> IO BtDefaultVehicleRaycaster
forall p0.
BtDynamicsWorldClass p0 =>
p0 -> IO BtDefaultVehicleRaycaster
btDefaultVehicleRaycaster bc
dw
    BtRaycastVehicle
vehicle <- BtRaycastVehicle_btVehicleTuning
-> BtRigidBody -> BtDefaultVehicleRaycaster -> IO BtRaycastVehicle
forall p0 p1 p2.
(BtRaycastVehicle_btVehicleTuningClass p0, BtRigidBodyClass p1,
 BtVehicleRaycasterClass p2) =>
p0 -> p1 -> p2 -> IO BtRaycastVehicle
btRaycastVehicle BtRaycastVehicle_btVehicleTuning
tuning BtRigidBody
carChassis BtDefaultVehicleRaycaster
vehicleRayCaster
    BtRigidBody -> Int -> IO ()
forall bc. BtCollisionObjectClass bc => bc -> Int -> IO ()
btCollisionObject_setActivationState BtRigidBody
carChassis 4 -- #define DISABLE_DEACTIVATION 4
    bc -> BtRaycastVehicle -> IO ()
forall bc p0.
(BtDynamicsWorldClass bc, BtActionInterfaceClass p0) =>
bc -> p0 -> IO ()
btDynamicsWorld_addVehicle bc
dw BtRaycastVehicle
vehicle
{-
#ifdef FORCE_ZAXIS_UP
		int rightIndex = 0; 
		int upIndex = 2; 
		int forwardIndex = 1;
		btVector3 wheelDirectionCS0(0,0,-1);
		btVector3 wheelAxleCS(1,0,0);
#else
		int rightIndex = 0;
		int upIndex = 1;
		int forwardIndex = 2;
		btVector3 wheelDirectionCS0(0,-1,0);
		btVector3 wheelAxleCS(-1,0,0);
#endif
-}
    BtRaycastVehicle -> Int -> Int -> Int -> IO ()
forall bc.
BtRaycastVehicleClass bc =>
bc -> Int -> Int -> Int -> IO ()
btRaycastVehicle_setCoordinateSystem BtRaycastVehicle
vehicle 0 1 2
    let connectionHeight :: Float
connectionHeight        = 0.0
        cCUBE_HALF_EXTENTS :: Float
cCUBE_HALF_EXTENTS      = 1.2
        wheelDirectionCS0 :: Vec3
wheelDirectionCS0       = Float -> Float -> Float -> Vec3
Vec3 0 (-1) 0
        wheelAxleCS :: Vec3
wheelAxleCS             = Float -> Float -> Float -> Vec3
Vec3 (-1) 0 0
        suspensionRestLength :: Float
suspensionRestLength    = 0.6
        suspensionStiffness :: Float
suspensionStiffness     = 20
        suspensionDamping :: Float
suspensionDamping       = 2.3
        suspensionCompression :: Float
suspensionCompression   = 4.4
        rollInfluence :: Float
rollInfluence           = 0.1
        wheelFriction :: Float
wheelFriction           = 1000
        
    BtRaycastVehicle
-> Vec3
-> Vec3
-> Vec3
-> Float
-> Float
-> BtRaycastVehicle_btVehicleTuning
-> Bool
-> IO (BtWheelInfo, Vec3, Vec3, Vec3)
forall bc p5.
(BtRaycastVehicleClass bc,
 BtRaycastVehicle_btVehicleTuningClass p5) =>
bc
-> Vec3
-> Vec3
-> Vec3
-> Float
-> Float
-> p5
-> Bool
-> IO (BtWheelInfo, Vec3, Vec3, Vec3)
btRaycastVehicle_addWheel BtRaycastVehicle
vehicle 
                                (Float -> Float -> Float -> Vec3
Vec3 (Float
cCUBE_HALF_EXTENTSFloat -> Float -> Float
forall a. Num a => a -> a -> a
-(0.3Float -> Float -> Float
forall a. Num a => a -> a -> a
*Float
wheelWidth)) Float
connectionHeight (2Float -> Float -> Float
forall a. Num a => a -> a -> a
*Float
cCUBE_HALF_EXTENTSFloat -> Float -> Float
forall a. Num a => a -> a -> a
-Float
wheelRadius))
                                Vec3
wheelDirectionCS0 Vec3
wheelAxleCS Float
suspensionRestLength Float
wheelRadius BtRaycastVehicle_btVehicleTuning
tuning Bool
True

    BtRaycastVehicle
-> Vec3
-> Vec3
-> Vec3
-> Float
-> Float
-> BtRaycastVehicle_btVehicleTuning
-> Bool
-> IO (BtWheelInfo, Vec3, Vec3, Vec3)
forall bc p5.
(BtRaycastVehicleClass bc,
 BtRaycastVehicle_btVehicleTuningClass p5) =>
bc
-> Vec3
-> Vec3
-> Vec3
-> Float
-> Float
-> p5
-> Bool
-> IO (BtWheelInfo, Vec3, Vec3, Vec3)
btRaycastVehicle_addWheel BtRaycastVehicle
vehicle 
                                (Float -> Float -> Float -> Vec3
Vec3 (-Float
cCUBE_HALF_EXTENTSFloat -> Float -> Float
forall a. Num a => a -> a -> a
+(0.3Float -> Float -> Float
forall a. Num a => a -> a -> a
*Float
wheelWidth)) Float
connectionHeight (2Float -> Float -> Float
forall a. Num a => a -> a -> a
*Float
cCUBE_HALF_EXTENTSFloat -> Float -> Float
forall a. Num a => a -> a -> a
-Float
wheelRadius))
                                Vec3
wheelDirectionCS0 Vec3
wheelAxleCS Float
suspensionRestLength Float
wheelRadius BtRaycastVehicle_btVehicleTuning
tuning Bool
True

    BtRaycastVehicle
-> Vec3
-> Vec3
-> Vec3
-> Float
-> Float
-> BtRaycastVehicle_btVehicleTuning
-> Bool
-> IO (BtWheelInfo, Vec3, Vec3, Vec3)
forall bc p5.
(BtRaycastVehicleClass bc,
 BtRaycastVehicle_btVehicleTuningClass p5) =>
bc
-> Vec3
-> Vec3
-> Vec3
-> Float
-> Float
-> p5
-> Bool
-> IO (BtWheelInfo, Vec3, Vec3, Vec3)
btRaycastVehicle_addWheel BtRaycastVehicle
vehicle 
                                (Float -> Float -> Float -> Vec3
Vec3 (-Float
cCUBE_HALF_EXTENTSFloat -> Float -> Float
forall a. Num a => a -> a -> a
+(0.3Float -> Float -> Float
forall a. Num a => a -> a -> a
*Float
wheelWidth)) Float
connectionHeight (-2Float -> Float -> Float
forall a. Num a => a -> a -> a
*Float
cCUBE_HALF_EXTENTSFloat -> Float -> Float
forall a. Num a => a -> a -> a
+Float
wheelRadius))
                                Vec3
wheelDirectionCS0 Vec3
wheelAxleCS Float
suspensionRestLength Float
wheelRadius BtRaycastVehicle_btVehicleTuning
tuning Bool
False

    BtRaycastVehicle
-> Vec3
-> Vec3
-> Vec3
-> Float
-> Float
-> BtRaycastVehicle_btVehicleTuning
-> Bool
-> IO (BtWheelInfo, Vec3, Vec3, Vec3)
forall bc p5.
(BtRaycastVehicleClass bc,
 BtRaycastVehicle_btVehicleTuningClass p5) =>
bc
-> Vec3
-> Vec3
-> Vec3
-> Float
-> Float
-> p5
-> Bool
-> IO (BtWheelInfo, Vec3, Vec3, Vec3)
btRaycastVehicle_addWheel BtRaycastVehicle
vehicle 
                                (Float -> Float -> Float -> Vec3
Vec3 (Float
cCUBE_HALF_EXTENTSFloat -> Float -> Float
forall a. Num a => a -> a -> a
-(0.3Float -> Float -> Float
forall a. Num a => a -> a -> a
*Float
wheelWidth)) Float
connectionHeight (-2Float -> Float -> Float
forall a. Num a => a -> a -> a
*Float
cCUBE_HALF_EXTENTSFloat -> Float -> Float
forall a. Num a => a -> a -> a
+Float
wheelRadius))
                                Vec3
wheelDirectionCS0 Vec3
wheelAxleCS Float
suspensionRestLength Float
wheelRadius BtRaycastVehicle_btVehicleTuning
tuning Bool
False
    
    Int
numWheels <- BtRaycastVehicle -> IO Int
forall bc. BtRaycastVehicleClass bc => bc -> IO Int
btRaycastVehicle_getNumWheels BtRaycastVehicle
vehicle
    [Int] -> (Int -> IO ()) -> IO ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
t a -> (a -> m b) -> m ()
M.forM_ [0..Int
numWheelsInt -> Int -> Int
forall a. Num a => a -> a -> a
-1] ((Int -> IO ()) -> IO ()) -> (Int -> IO ()) -> IO ()
forall a b. (a -> b) -> a -> b
$ \i :: Int
i -> do
        BtWheelInfo
wheel <- BtRaycastVehicle -> Int -> IO BtWheelInfo
forall bc. BtRaycastVehicleClass bc => bc -> Int -> IO BtWheelInfo
btRaycastVehicle_getWheelInfo BtRaycastVehicle
vehicle Int
i
        BtWheelInfo -> Float -> IO ()
forall bc. BtWheelInfoClass bc => bc -> Float -> IO ()
btWheelInfo_m_suspensionStiffness_set BtWheelInfo
wheel Float
suspensionStiffness
        BtWheelInfo -> Float -> IO ()
forall bc. BtWheelInfoClass bc => bc -> Float -> IO ()
btWheelInfo_m_wheelsDampingRelaxation_set BtWheelInfo
wheel Float
suspensionDamping
        BtWheelInfo -> Float -> IO ()
forall bc. BtWheelInfoClass bc => bc -> Float -> IO ()
btWheelInfo_m_wheelsDampingCompression_set BtWheelInfo
wheel Float
suspensionCompression
        BtWheelInfo -> Float -> IO ()
forall bc. BtWheelInfoClass bc => bc -> Float -> IO ()
btWheelInfo_m_frictionSlip_set BtWheelInfo
wheel Float
wheelFriction
        BtWheelInfo -> Float -> IO ()
forall bc. BtWheelInfoClass bc => bc -> Float -> IO ()
btWheelInfo_m_rollInfluence_set BtWheelInfo
wheel Float
rollInfluence
    [Int] -> (Int -> IO ()) -> IO ()
forall (t :: * -> *) (m :: * -> *) a b.
(Foldable t, Monad m) =>
t a -> (a -> m b) -> m ()
M.forM_ [0..Int
numWheelsInt -> Int -> Int
forall a. Num a => a -> a -> a
-1] ((Int -> IO ()) -> IO ()) -> (Int -> IO ()) -> IO ()
forall a b. (a -> b) -> a -> b
$ \i :: Int
i -> do
        BtWheelInfo
wheel <- BtRaycastVehicle -> Int -> IO BtWheelInfo
forall bc. BtRaycastVehicleClass bc => bc -> Int -> IO BtWheelInfo
btRaycastVehicle_getWheelInfo BtRaycastVehicle
vehicle Int
i
        Float -> IO ()
forall a. Show a => a -> IO ()
print (Float -> IO ()) -> IO Float -> IO ()
forall (m :: * -> *) a b. Monad m => (a -> m b) -> m a -> m b
=<< BtWheelInfo -> IO Float
forall bc. BtWheelInfoClass bc => bc -> IO Float
btWheelInfo_m_suspensionStiffness_get BtWheelInfo
wheel
        Float -> IO ()
forall a. Show a => a -> IO ()
print (Float -> IO ()) -> IO Float -> IO ()
forall (m :: * -> *) a b. Monad m => (a -> m b) -> m a -> m b
=<< BtWheelInfo -> IO Float
forall bc. BtWheelInfoClass bc => bc -> IO Float
btWheelInfo_m_wheelsDampingRelaxation_get BtWheelInfo
wheel
        Float -> IO ()
forall a. Show a => a -> IO ()
print (Float -> IO ()) -> IO Float -> IO ()
forall (m :: * -> *) a b. Monad m => (a -> m b) -> m a -> m b
=<< BtWheelInfo -> IO Float
forall bc. BtWheelInfoClass bc => bc -> IO Float
btWheelInfo_m_wheelsDampingCompression_get BtWheelInfo
wheel
        Float -> IO ()
forall a. Show a => a -> IO ()
print (Float -> IO ()) -> IO Float -> IO ()
forall (m :: * -> *) a b. Monad m => (a -> m b) -> m a -> m b
=<< BtWheelInfo -> IO Float
forall bc. BtWheelInfoClass bc => bc -> IO Float
btWheelInfo_m_frictionSlip_get BtWheelInfo
wheel
        Float -> IO ()
forall a. Show a => a -> IO ()
print (Float -> IO ()) -> IO Float -> IO ()
forall (m :: * -> *) a b. Monad m => (a -> m b) -> m a -> m b
=<< BtWheelInfo -> IO Float
forall bc. BtWheelInfoClass bc => bc -> IO Float
btWheelInfo_m_rollInfluence_get BtWheelInfo
wheel
    (BtDefaultMotionState, BtRigidBody, BtRaycastVehicle)
-> IO (BtDefaultMotionState, BtRigidBody, BtRaycastVehicle)
forall (m :: * -> *) a. Monad m => a -> m a
return (BtDefaultMotionState
carMotionSate,BtRigidBody
carChassis,BtRaycastVehicle
vehicle)