--- a/c/Motion.cpp	Fri Aug 19 12:15:23 2011 -0400
+++ b/c/Motion.cpp	Tue Aug 23 13:14:47 2011 -0400
@@ -1,4 +1,5 @@
 #include "Motion.hpp"
+#include "cvutils.hpp"
 
 #include "opencv2/core/core.hpp"
 #include "opencv2/highgui/highgui.hpp"
@@ -24,9 +25,28 @@
   return result;
 }
 
+bool FeatureTrajectory::motionSmooth(const int& accelerationBound, const int& deviationBound) const {
+  bool result = true;
+  unsigned int nPositions = positions.size();
+  if (nPositions >= 3) {
+    float ratio;
+    if (displacementDistances[nPositions-2] > displacementDistances[nPositions-3])
+      ratio = displacementDistances[nPositions-2] / displacementDistances[nPositions-3];
+    else
+      ratio = displacementDistances[nPositions-3] / displacementDistances[nPositions-2];
+
+    float cosine = scalarProduct(velocities[nPositions-3],velocities[nPositions-2]) / (displacementDistances[nPositions-3] * displacementDistances[nPositions-2]);
+    
+    result &= (ratio < accelerationBound) & (cosine > deviationBound);
+  }
+  return result;
+}
+
 void FeatureTrajectory::addPoint(const int& frameNum, const Point2f& p) {
   positions.add(frameNum, p);
   computeMotionData(frameNum);
+  assert(positions.size() == displacementDistances.size()+1);
+  assert(positions.size() == velocities.size()+1);
 }
 
 void FeatureTrajectory::shorten(void) { 
@@ -56,10 +76,10 @@
 
 void FeatureTrajectory::computeMotionData(const int& frameNum) {
   unsigned int nPositions = positions.size();
-  if (nPositions >= 3) {
+  if (nPositions >= 2) {
     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);
+    //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);