Implement ORCA::GetCost() correctly
Currently, ORCA only works correctly when using global optimization.
To allow people to use this cost function in a "sampling" or "gradient" policy, we need to implement and test the GetCost() function. I have made a first implementation based on intuition, but it clearly does not produce the correct results yet.