Refine Cost Calculation in C3 Library

[Meow404]

Implement an improved cost calculation algorithm in the C3 repository. Ensure code is documented, with unit tests, Python bindings, README updates, and passes Drake's clang autoformatter.

Assignee: @ebianchi