--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/c/Motion.cpp	Thu Aug 18 22:25:21 2011 -0400
@@ -0,0 +1,61 @@
+#include "Motion.hpp"
+
+#include "opencv2/core/core.hpp"
+#include "opencv2/highgui/highgui.hpp"
+
+#include "src/TrajectoryDBAccessList.h"
+
+using namespace std;
+using namespace cv;
+
+FeatureTrajectory::FeatureTrajectory(const int& frameNum, const cv::Point2f& p) {
+  addPoint(frameNum, p);
+}
+
+bool FeatureTrajectory::largeDisplacement(const unsigned 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);
+}
+
+void FeatureTrajectory::write(TrajectoryDBAccess<Point2f>& trajectoryDB) const {
+  /// \todo save velocities
+  trajectoryDB.write(positions);
+}
+
+#ifdef USE_OPENCV
+/// \todo add option for anti-aliased drawing, thickness
+void FeatureTrajectory::draw(Mat& img, const Scalar& color) const {
+  Point2f p1 = positions[0];
+  for (unsigned int i=1; i<positions.size(); i++) {
+    Point2f p2 = positions[i];
+    line(img, p1, p2, color, 1);
+    p1 = p2;
+  }
+}
+#endif
+
+// protected
+
+void FeatureTrajectory::computeMotionData(const int& frameNum) {
+  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);
+  }
+}