|
发表于 2015-8-10 11:44:23
|
显示全部楼层
算法就是这么几行?
public void PushPoint(Point pnt)
{
if (_filteredPosition.HasValue)
{
Point old = _filteredPosition.Value;
Vector delta = new Vector(pnt.X - old.X, pnt.Y - old.Y);
// 下面是滤波算法。
_vectorsSum += delta;
_vectorsCount++;
if (_vectorsCount >= _maxCount || _vectorsSum.Length >= _sumLength)
{
delta = _vectorsSum / _vectorsCount;
_filteredPosition = new Point(old.X + delta.X, old.Y + delta.Y);
_vectorsSum = new Vector();
_vectorsCount = 0;
}
}
else
{
_filteredPosition = pnt;
_vectorsSum = new Vector();
_vectorsCount = 0;
}
} |
|