Implementation of non phisical collision, collisions counter and sotore to Matlab dat.

Signed-off-by: mitkof6 <jimstanev@gmail.com>