public void Add(ContactPoint cp)
        {
            if (_maxIndex >= MaxPoints)
            {
                return;
            }

            _points.SetItem(++_maxIndex, cp);
        }