--- a/c/Feature.cpp	Wed Aug 17 23:21:26 2011 -0400
+++ b/c/Feature.cpp	Thu Aug 18 01:03:38 2011 -0400
@@ -12,6 +12,18 @@
   addPoint(frameNum, p);
 }
 
+bool FeatureTrajectory::largeDisplacement(const int& nDisplacements, const float& minTotalFeatureDisplacement) const {
+  bool result = true;
+  unsigned int nPositions = positions.size();
+  if (nPositions > nDisplacements) {
+    float disp = 0;
+    for (int i=0; i<nDisplacements; i++)
+      disp += displacementDistances[nPositions-2-i];
+    result = disp > minTotalFeatureDisplacement;
+  }
+  return result;
+}
+
 void FeatureTrajectory::addPoint(const int& frameNum, const Point2f& p) {
   positions.add(frameNum, p);
   computeMotionData(frameNum);
@@ -40,15 +52,10 @@
   unsigned int nPositions = positions.size();
   if (nPositions >= 3) {
     Point2f displacement = positions[nPositions-1] - positions[nPositions-2];
+    if (nPositions == 2) // duplicate first displacement so that positions and velocities have the same length
+      velocities.add(frameNum-1, displacement);
     velocities.add(frameNum, displacement);
     float dist = norm(displacement);
     displacementDistances.push_back(dist);
-  } else if (nPositions == 2) {
-    Point2f displacement = positions[1] - positions[0];
-    velocities.add(frameNum-1, displacement);
-    velocities.add(frameNum, displacement);
-    float dist = norm(displacement);
-    displacementDistances.push_back(dist);
-    displacementDistances.push_back(dist);
   }
 }