[Review]: Fix PC matrix calculation from CD matrix.

This fixes a bug with SWAP files.

@rhewett @ayshih can you check this maths?

Todo:

* [x] Add SWAP test file.