Added some math functions to CoDyCo lib