{-
   Haskell Dynamics Engine, Copyright (C) 2007 Ruben Henner Zilibowitz
   All rights reserved. Email: rubenz@cse.unsw.edu.au Web: www.cse.unsw.edu.au/~rubenz
   
   This is free software; you can redistribute it and/or modify it
   under the terms of the GNU General Public License that is include with this
   package in the file LICENSE-GPL.TXT.
-}

-- Dynamics
-- Created by Ruben Henner Zilibowitz 18/3/2007

module Dynamics where

import Quaternions
import Matrix3x3
import Data.Maybe
import Data.Array
import LCP (compute_forces,compute_forces_with_holonomic)
--import QuadProgInterface(solve_simple_qp)
import Objects
import World
import Gausselim(gauss)
import Collisions(findAllContacts)
import Data.List(insertBy,sortBy,nubBy)
import SeparatingLine
--import Debug.Trace(trace)

---
--- stepWorld
---

--- Every time one modifies the state variables of a rigid body object
--- one should pass it to this function to compute the auxiliary variables
--- which depend on the state variables.
computeAuxiliaryVars :: Body -> Body
computeAuxiliaryVars body = body { invInertia = theInvInertia,
                                   rotationMat = theRotMat,
                                   velocity = (momentum body) .*. (invMass body),
                                   rotationalVel = theInvInertia `matXvec` (angularMomentum body) }
   where theRotMat = rotMatrix (orientation body)
         theInvInertia = theRotMat*(invInertiaBody body)*(transposeMat theRotMat)

-- like map but for arrays
mapArray :: (Ix i) => (a -> b) -> Array i a -> Array i b
mapArray f arr = array bnds [(i,f (arr!i)) | i <- is]
   where bnds = bounds arr
         is = indices arr

type Line = (Vector RealNum,Vector RealNum)

{-
There are different heuristic functions for computing
the best separating line. Some possibilities are commented out
below. One can try them all out to see which works best.
-}
separatingLineFunction :: [Vector RealNum] -> Line
separatingLineFunction = lineOfMaxVariance  -- this line maximises the variance of projected points onto the line
--separatingLineFunction = lineOfSpan  -- this is one diagonal of the bounding box of all points
--separatingLineFunction = const (0,Vec 1 0 0)  -- this is a fixed line

lineOfSpan :: [Vector RealNum] -> Line
lineOfSpan positions = (minPos,Matrix3x3.normalise (maxPos - minPos))
   where (minPos,maxPos) = (foldl1 minVectorComponents positions,foldl1 maxVectorComponents positions)

-- simulates a step through a given amount of time dt of the world
stepWorld :: RealNum -> World -> World
stepWorld dt w@(World_ bodies islands joints jointGroups sortedIntervals gravity restitution colours) = World_ finalBodies islands joints jointGroups sortedIntervals' gravity restitution colours
   where contacts = {-trace "findAllContacts"-}
                    (_scc_ "findAllContacts" (findAllContacts w))
         bodies' = {-trace "foldl solveAllJointImpulses"-}
                   (_scc_ "foldl_solveAllJointImpulses" (foldl solveAllJointImpulses bodies jointGroups))
         bodies'' = {-trace "computeImpulsesIteratively"-}
                    (_scc_ "computeImpulsesIteratively"
                       (computeImpulsesIteratively maxImpulseIterations bodies' contacts restitution))
         jointsContactsGroups = {-trace "makeJointGroups"-}
                                (_scc_ "makeJointGroups" (makeJointGroups bodies'' (joints ++ contacts)))
         bodies''' = {-trace "foldl computeForces"-}
                     (_scc_ "foldl_computeForces" (foldl computeForces bodies'' jointsContactsGroups))
         finalBodies = {-trace "finalBodies"-}
                       (mapArray (stepBody dt gravity) bodies''')
         positions = {-trace "positions"-}
                     [position (bodies!i) | i <- (indices bodies), not (isPlane (bodies!i))]
         sortedIntervals' = {-trace "sortedIntervals'"-}
                            ({-insertionSortBy-}sortBy compareSnds
                               (updateIntervals finalBodies sortedIntervals (separatingLineFunction positions)))

isPlane (Body_ (Plane _ _ _) _ _ _ _ _ _ _ _ _ _ _ _ _ _ _) = True
isPlane _ = False

updateIntervals bodies intervals separatingLine = [(i,intervalOnLine (bodies!i) separatingLine) | (i,_) <- intervals]

insertionSortBy _ [] = []
insertionSortBy cmp (x:xs) = insertBy cmp x (insertionSortBy cmp xs)

-- integrates kinematics equations for a given body
stepBody :: RealNum -> Vector RealNum -> Body -> Body
stepBody dt gravity body
   | (anchored body) = body
   | otherwise = computeAuxiliaryVars
                    body { position = (position body)+(velocity body).*.dt,
                        orientation = Quaternions.normalise ((orientation body)+((vec2quat ((rotationalVel body).*.(dt*0.5)))*(orientation body))),
                        momentum = (momentum body) + (force body).*.dt,
                        angularMomentum = (angularMomentum body) + (torque body).*.dt,
                        force = gravity .*. (mass body),
                        torque = 0 }

---
--- for solving joints problem
---
solveAllJointImpulses bodies joints = bodies'
   where
      mat = compute_joint_mass_matrix bodies joints
      vec = compute_joint_velocity_vector bodies joints
      pvec = listToVecs (gauss mat vec)
      bodies' = accum applyImpulse bodies
                   (concat [[(a,((orientation (bodies!a)).*pa,p)),
                             (b,((orientation (bodies!b)).*pb,-p))]
                      | (Joint_ a b (BallSocket pa pb),p) <- zip joints pvec])

applyImpulse body (loc,impulse) = computeAuxiliaryVars
                                     (body {momentum = (momentum body) + impulse,
                                            angularMomentum = (angularMomentum body) + (cross loc impulse)})

-- groups each successive triplet of values into a vector
listToVecs [] = []
listToVecs (a:b:c:rest) = (Vec a b c) : (listToVecs rest)
listToVecs _ = error "listToVecs: list must have length which is a multiple of three"

-- error reduction parameter
-- This tries to correct errors in the constraints. Too high and wierd stuff might happen.
-- Too low and errors are likely going to accumulate. This is probably not a good way to deal
-- with this problem but it is a cheap and easy trick to use for now.
erp = 2.5

-- vector containing velocity data of all joints
compute_joint_velocity_vector bodies joints = concatMap (vecToList.(compute_joint_velocity_term bodies)) joints
compute_joint_velocity_term bodies (Joint_ a b (BallSocket pa pb)) = negate ((velA - velB) + (locA - locB).*.erp)
   where
      bodyA = bodies ! a
      bodyB = bodies ! b
      ra = (orientation bodyA) .* pa
      rb = (orientation bodyB) .* pb
      locA = (position bodyA) + ra
      locB = (position bodyB) + rb
      velA = (velocity bodyA) + (cross (rotationalVel bodyA) ra)
      velB = (velocity bodyB) + (cross (rotationalVel bodyB) rb)

-- matrix containing mass and inertia data for all pairs of joints
compute_joint_mass_matrix bodies joints = catMatrices [[compute_joint_matrix_term bodies ci cj | cj <- joints] | ci <- joints]

-- computes the effect joint j  has on joint i
compute_joint_matrix_term bodies (Joint_ ai bi (BallSocket pai pbi)) (Joint_ aj bj (BallSocket paj pbj))
   | (not (ai == aj || bi == bj || ai == bj || bi == aj)) = 0
   | otherwise = (scaleMatrix a_linear) + a_angular + (scaleMatrix b_linear) + b_angular
   where
      bodyAi = bodies ! ai
      bodyBi = bodies ! bi
      bodyAj = bodies ! aj
      bodyBj = bodies ! bj
      rai = (orientation bodyAi) .* pai
      rbi = (orientation bodyBi) .* pbi
      raj = (orientation bodyAj) .* paj
      rbj = (orientation bodyBj) .* pbj
      (a_linear,a_angular) =
         if (aj == ai) then
            (invMass bodyAi,(crossMatrix2 rai)*(invInertia bodyAi)*(crossMatrix raj))
         else (if (bj == ai) then
            (negate (invMass bodyAi),(crossMatrix rai)*(invInertia bodyAi)*(crossMatrix rbj))
         else (0,0))
      (b_linear,b_angular) =
         if (aj == bi) then
            (negate (invMass bodyBi),(crossMatrix rbi)*(invInertia bodyBi)*(crossMatrix raj))
         else (if (bj == bi) then
            (invMass bodyBi,(crossMatrix2 rbi)*(invInertia bodyBi)*(crossMatrix rbj))
         else (0,0))

applyJointForce body (loc,f) = computeAuxiliaryVars
                                     (body {force = (force body) + f,
                                            torque = (torque body) + (cross loc f)})

-- vector containing acceleration data for all joints
compute_joint_acceleration_vector bodies joints = concatMap (vecToList.(compute_joint_acceleration_term bodies)) joints
compute_joint_acceleration_term bodies (Joint_ a b (BallSocket pa pb)) = negate ((pa_acc - pb_acc) + (locA - locB).*.erp)
   where
      bodyA = bodies ! a
      bodyB = bodies ! b
      ra = (orientation bodyA) .* pa
      rb = (orientation bodyB) .* pb
      locA = (position bodyA) + ra
      locB = (position bodyB) + rb
      pa_acc = (force bodyA).*.(invMass bodyA) + (cross ((invInertia bodyA) `matXvec` ((torque bodyA) + ((angularMomentum bodyA) `cross` (rotationalVel bodyA)))) ra) + (cross (rotationalVel bodyA) (cross (rotationalVel bodyA) ra))
      pb_acc = (force bodyB).*.(invMass bodyB) + (cross ((invInertia bodyB) `matXvec` ((torque bodyB) + ((angularMomentum bodyB) `cross` (rotationalVel bodyB)))) rb) + (cross (rotationalVel bodyB) (cross (rotationalVel bodyB) rb))

---
--- for solving colliding contacts problem
---

-- maximum iterations before exiting loop to find a fixed point in computing
-- contact impulses
maxImpulseIterations = 1000

-- uses iteration to fixed point to solve for contact impulses
computeImpulsesIteratively :: Int -> Bodies -> [Joint] -> RealNum -> Bodies
computeImpulsesIteratively 0 bodies _ _ = bodies
computeImpulsesIteratively n bodies contacts restitution
   | (null collidingContacts) = bodies
   | otherwise = computeImpulsesIteratively (n-1) (foldl (resolveCollision restitution) bodies collidingContacts) contacts restitution
   where collidingContacts = [c | c <- contacts, contactRelNormVel bodies c < 0]

{-
-- an m by n matrix of zeroes
zero_mat m n = replicate m (replicate n 0)

computeImpulses :: Bodies -> [Contact] -> [Joint] -> Bodies
computeImpulses bodies [] _ = bodies
computeImpulses bodies contacts _ = bodies'
   where
      matG = compute_mass_matrix bodies contacts
      g0 = map (0.5 *) (compute_velocity_vector bodies contacts)
      jvec = maybe (error "Failed to solve QP problem") snd (solve_simple_qp matG g0)
      bodies' = accum applyImpulse bodies (concat [[(a,(loc,norm,depth,j)),(b,(loc,norm,depth,-j))] |
                                                    (Contact_ a b loc norm depth,j) <- zip contacts jvec])

applyImpulse body (loc,norm,depth,j) = let jnorm = norm .*. j
                                       in computeAuxiliaryVars
                                             body {momentum = (momentum body) + jnorm,
                                                   angularMomentum = (angularMomentum body) + (cross (loc - (position body)) jnorm)}
-}

-- the relative normal velocity at a contact point
contactRelNormVel bodies (Joint_ a b (Contact loc norm depth)) = dot norm ((pt_velocity (bodies ! a) loc) - (pt_velocity (bodies ! b) loc))
{-
compute_velocity_vector bodies contacts = map (compute_contact_velocity_term bodies) contacts
compute_contact_velocity_term bodies (Contact_ a b loc norm depth) =
   if relNormVel < 0 then (1 + restitution) * relNormVel else relNormVel
   where relNormVel = dot norm ((pt_velocity (bodies ! a) loc) - (pt_velocity (bodies ! b) loc))
-}

-- the mass data for all pairs of contacts
compute_mass_matrix bodies contacts = [[compute_mass_term bodies ci cj | cj <- contacts] | ci <- contacts]
compute_mass_term bodies (Joint_ ai bi (Contact pi ni _)) (Joint_ aj bj (Contact pj nj _))
   | (not (ai == aj || bi == bj || ai == bj || bi == aj)) = 0
   | otherwise = dot ni ((a_linear + a_angular) - (b_linear + b_angular))
   where
      bodyA = bodies ! ai
      bodyB = bodies ! bi
      ria = pi - (position bodyA)
      rib = pi - (position bodyB)
      rja = pj - (position bodyA)
      rjb = pj - (position bodyB)
      (impulse_on_a,angImpulse_on_a) = if (aj == ai) then (nj,cross rja nj) else (if (bj == ai) then (-nj,cross rja (-nj)) else (0,0))
      (impulse_on_b,angImpulse_on_b) = if (aj == bi) then (nj,cross rjb nj) else (if (bj == bi) then (-nj,cross rjb (-nj)) else (0,0))
      a_linear = impulse_on_a .*. (invMass bodyA)
      a_angular = cross ((invInertia bodyA) `matXvec` angImpulse_on_a) ria
      b_linear = impulse_on_b .*. (invMass bodyB)
      b_angular = cross ((invInertia bodyB) `matXvec` angImpulse_on_b) rib

---
--- for solving resting contacts problem
---

contact_to_joint_effect :: Bodies -> Joint -> Joint -> Vector RealNum
contact_to_joint_effect bodies (Joint_ ai bi (BallSocket pai pbi)) (Joint_ aj bj (Contact pos norm _))
   | (not (ai == aj || bi == bj || ai == bj || bi == aj)) = 0
   | otherwise = (a_linear + a_angular) - (b_linear + b_angular)
   where
      bodyAi = bodies ! ai
      bodyBi = bodies ! bi
      bodyAj = bodies ! aj
      bodyBj = bodies ! bj
      rai = (orientation bodyAi) .* pai
      rbi = (orientation bodyBi) .* pbi
      raj = pos - (position bodyAi)
      rbj = pos - (position bodyBi)
      (a_linear,a_angular) =
         if (aj == ai) then
            (norm.*.(invMass bodyAi),((invInertia bodyAj) `matXvec` (raj `cross` norm)) `cross` rai)
         else (if (bj == ai) then
            ((-norm).*.(invMass bodyAi),((invInertia bodyBj) `matXvec` (rbj `cross` (-norm))) `cross` rai)
         else (0,0))
      (b_linear,b_angular) =
         if (aj == bi) then
            ((-norm).*.(invMass bodyBi),((invInertia bodyAj) `matXvec` (raj `cross` (-norm))) `cross` rbi)
         else (if (bj == bi) then
            (norm.*.(invMass bodyBi),((invInertia bodyBj) `matXvec` (rbj `cross` norm)) `cross` rbi)
         else (0,0))

joint_to_contact_effect :: Bodies -> Joint -> Joint -> Vector RealNum
joint_to_contact_effect bodies (Joint_ ai bi (Contact pos norm _)) (Joint_ aj bj (BallSocket paj pbj))
   | (not (ai == aj || bi == bj || ai == bj || bi == aj)) = 0
   | otherwise = (a_linear + a_angular) - (b_linear + b_angular)
   where
      bodyAi = bodies ! ai
      bodyBi = bodies ! bi
      bodyAj = bodies ! aj
      bodyBj = bodies ! bj
      rai = pos - (position bodyAj)
      rbi = pos - (position bodyBj)
      raj = (orientation bodyAj) .* paj
      rbj = (orientation bodyBj) .* pbj
      (a_linear,a_angular) =
         if (aj == ai) then
            (norm.*.(invMass bodyAi),((crossMatrix2 rai)*(invInertia bodyAi)*(crossMatrix raj)) `matXvec` norm)
         else (if (bj == ai) then
            ((-norm).*.(invMass bodyAi),((crossMatrix2 rai)*(invInertia bodyAi)*(crossMatrix rbj)) `matXvec` (-norm))
         else (0,0))
      (b_linear,b_angular) =
         if (aj == bi) then
            ((-norm).*.(invMass bodyBi),((crossMatrix2 rbi)*(invInertia bodyBi)*(crossMatrix raj)) `matXvec` (-norm))
         else (if (bj == bi) then
            (norm.*.(invMass bodyBi),((crossMatrix2 rbi)*(invInertia bodyBi)*(crossMatrix rbj)) `matXvec` norm)
         else (0,0))

-- matrix containing mass data for all contacts and joints together
compute_big_mass_matrix :: Bodies -> [Joint] -> [Joint] -> [[RealNum]]
compute_big_mass_matrix bodies contacts joints = (zipWith (++) matJJ matCJ) ++ (zipWith (++) matJC matCC)
   where
      matJJ = catMatrices [[compute_joint_matrix_term bodies ci cj | cj <- joints] | ci <- joints]
      matCC = [[compute_mass_term bodies ci cj | cj <- contacts] | ci <- contacts]
      matJC = catVectors1x3 [[joint_to_contact_effect bodies ci cj | cj <- joints] | ci <- contacts]
      matCJ = catVectors3x1 [[contact_to_joint_effect bodies ci cj | cj <- contacts] | ci <- joints]

catVectors3x1 mat = concatMap (map vecToList) mat
catVectors1x3 mat = map (concatMap vecToList) mat
{-
computeContactForces :: Bodies -> [Contact] -> [Joint] -> Bodies
computeContactForces bodies [] _ = bodies
computeContactForces bodies contacts _ = bodies'
   where
      restingContacts = [c | c <- contacts, abs (contactRelNormVel bodies c) < resting_contact_max_vel]
      matG = assertSquareMatrix (compute_mass_matrix bodies restingContacts)
      g0 = (compute_acceleration_vector bodies restingContacts)
      --fvec = maybe (error "Failed to solve QP problem") snd (solve_simple_qp matG g0)
      fvec = compute_forces matG g0
      bodies' = accum applyForce bodies (concat [[(a,(loc,norm,depth,f)),(b,(loc,norm,depth,-f))] | (c@(Contact_ a b loc norm depth),f) <- zip restingContacts fvec])
-}

minContactDist = 1e-3

-- solves for all forces at contacts and joints
computeForces :: Bodies -> [Joint] -> Bodies
computeForces bodies [] = bodies
computeForces bodies jointsContacts = bodies''
   where
      joints = [c | c@(Joint_ _ _ (BallSocket _ _)) <- jointsContacts]
      contacts = nubBy contactsAreClose [c | c@(Joint_ _ _ (Contact _ _ _)) <- jointsContacts]
      restingContacts = [c | c <- contacts, abs (contactRelNormVel bodies c) < resting_contact_max_vel]
      j0 = compute_joint_acceleration_vector bodies joints
      matG = assertSquareMatrix (compute_big_mass_matrix bodies restingContacts joints)
      g0 = (compute_acceleration_vector bodies restingContacts)
      --fvec = maybe (error "Failed to solve QP problem") snd (solve_simple_qp matG g0)
      n = length j0
      fvec = compute_forces_with_holonomic n matG (j0 ++ g0)
      (joints_fvec,contacts_fvec) = splitAt n fvec
      bodies' = accum applyForce bodies
                  (concat [[(a,(loc,norm,depth,f)),
                            (b,(loc,norm,depth,-f))]
                     | (c@(Joint_ a b (Contact loc norm depth)),f) <- zip restingContacts contacts_fvec])
      bodies'' = accum applyJointForce bodies'
                   (concat [[(a,((orientation (bodies'!a)).*pa,f)),
                             (b,((orientation (bodies'!b)).*pb,-f))]
                      | (Joint_ a b (BallSocket pa pb),f) <- zip joints (listToVecs joints_fvec)])

-- contacts are close is used to filter out contacts which are very near to each other
-- before solving the forces LCP
contactsAreClose :: Joint -> Joint -> Bool
contactsAreClose (Joint_ _ _ (Contact w _ _)) (Joint_ _ _ (Contact v _ _))
   = let x = v - w in (dot x x) < minContactDist

-- checks that a matrix is square
assertSquareMatrix :: [[a]] -> [[a]]
assertSquareMatrix mat | all (((length mat) ==).length) mat = mat
                     | otherwise = error "matrix is not square"

applyForce body (loc,norm,depth,f) = let fnorm = norm .*. f
                                     in body {force = (force body) + fnorm,
                                              torque = (torque body) + (cross (loc - (position body)) fnorm)}

-- vector containing acceleration data for all contacts
compute_acceleration_vector bodies contacts = map (compute_contact_acceleration_term bodies) contacts
compute_contact_acceleration_term bodies (Joint_ a b (Contact loc norm depth)) = bval
   where
      bodyA = bodies ! a
      bodyB = bodies ! b
      ra = loc - (position bodyA)
      rb = loc - (position bodyB)
      pa_acc = (force bodyA).*.(invMass bodyA) + (cross ((invInertia bodyA) `matXvec` ((torque bodyA) + ((angularMomentum bodyA) `cross` (rotationalVel bodyA)))) ra) + (cross (rotationalVel bodyA) (cross (rotationalVel bodyA) ra))
      pb_acc = (force bodyB).*.(invMass bodyB) + (cross ((invInertia bodyB) `matXvec` ((torque bodyB) + ((angularMomentum bodyB) `cross` (rotationalVel bodyB)))) rb) + (cross (rotationalVel bodyB) (cross (rotationalVel bodyB) rb))
      k1 = dot norm (pa_acc - pb_acc)
      -- This computation of ndot assumes that the normal is a face normal (the contact is vertex/face not edge/edge).
      -- I am not sure about the correct computation of ndot. wB X n is what Baraff's paper used (for vertex/face contact).
      ndot = cross (((rotationalVel bodyB) + (rotationalVel bodyA)).*.0.5) norm
--      ndot = cross (rotationalVel bodyB) norm
--      ndot = cross (((rotationalVel bodyB) - (rotationalVel bodyA)).*.0.5) norm
      p_rel_vel = (pt_velocity bodyA loc) - (pt_velocity bodyB loc)
      k2 = 2 * (dot ndot p_rel_vel)
      bval = k1 + k2

-- velocity of the given point on the given body
pt_velocity body pos = (velocity body) + (cross (rotationalVel body) (pos - (position body)))
------

-- converts a vector to a quaternion with zero real part
vec2quat :: (RealFloat a) => Vector a -> Quaternion a
vec2quat (Vec x y z) = Q 0 x y z

--separationFactor = -0.25
--microCollisionThreshold = 0
resting_contact_max_vel = 1e-9

-- solve impulse at a given contact
resolveCollision :: RealNum -> Bodies -> Joint -> Bodies
resolveCollision restitution world (Joint_ a b (Contact pos normal depth))
   | (v_rel_0 < 0) = world // [(a,bodyA'),(b,bodyB')]
   | otherwise = world
   where bodyA = world ! a
         bodyB = world ! b
         bodyA' = computeAuxiliaryVars bodyA {momentum = (momentum bodyA) + impulse,
                         angularMomentum = (angularMomentum bodyA) + (cross r_a impulse)}
         bodyB' = computeAuxiliaryVars bodyB {momentum = (momentum bodyB) - impulse,
                         angularMomentum = (angularMomentum bodyB) - (cross r_b impulse)}
         --e = if v_rel_0 < microCollisionThreshold then restitution else 1.0
         e = restitution
         j = -(1 + e) * v_rel_0 / k
         k = ((invMass bodyA) + (invMass bodyB) + (dot normal (cross ((invInertia bodyA) `matXvec` (cross r_a normal)) r_a))
                                                + (dot normal (cross ((invInertia bodyB) `matXvec` (cross r_b normal)) r_b)))
         r_a = pos - (position bodyA)
         r_b = pos - (position bodyB)
         p'_a = (velocity bodyA) + (cross (rotationalVel bodyA) r_a)
         p'_b = (velocity bodyB) + (cross (rotationalVel bodyB) r_b)
         v_rel_0 = dot normal (p'_a - p'_b)
         impulse = normal .*. j

{-
---
--- resolveOverlap
---
resolveOverlap :: Bodies -> Contact -> Bodies
resolveOverlap world (Contact_ i j pos normal depth) = world // [(i,bodyA'),(j,bodyB')]
   where bodyA = world ! i
         bodyB = world ! j
         bodyA' = computeAuxiliaryVars bodyA {position = (position bodyA) + seperation.*.(invMass bodyA),
                         orientation = Quaternions.normalise ((orientation bodyA) + (vec2quat ((matXvec (invInertia bodyA) (cross r_a seperation)).*.0.5) * (orientation bodyA)))}
         bodyB' = computeAuxiliaryVars bodyB {position = (position bodyB) + (-seperation).*.(invMass bodyB),
                         orientation = Quaternions.normalise ((orientation bodyB) + (vec2quat ((matXvec (invInertia bodyB) (cross r_b (-seperation))).*.0.5) * (orientation bodyB)))}
         s = separationFactor * depth / k
         k = ((invMass bodyA) + (invMass bodyB) + (dot normal (cross ((invInertia bodyA) `matXvec` (cross r_a normal)) r_a))
                                                + (dot normal (cross ((invInertia bodyB) `matXvec` (cross r_b normal)) r_b)))
         r_a = pos - (position bodyA)
         r_b = pos - (position bodyB)
         seperation = normal .*. s
-}