Upload new file

src/foot_alg.cpp

+#include "foot_alg.h"
+#include <math.h>
+
+FootAlgorithm::FootAlgorithm(void)
+{
+  pthread_mutex_init(&this->access_,NULL);
+}
+
+FootAlgorithm::~FootAlgorithm(void)
+{
+  pthread_mutex_destroy(&this->access_);
+}
+
+void FootAlgorithm::config_update(Config& config, uint32_t level)
+{
+  this->lock();
+
+  // save the current configuration
+  this->config_=config;
+  
+  this->unlock();
+}
+
+// FootAlgorithm Public API
+
+double FootAlgorithm::angle(tf::Vector3 p1, tf::Vector3 p2, tf::Vector3 p3, tf::Vector3 p4)
+{
+	const double PI = 3.14159265359.
+	double numleft = (p1.getX() * p2.getX()) + (p1.getY() * p2.getY()) + (p1.getZ() * p2.getZ());
+	double numright = (p3.getX() * p4.getX()) + (p3.getY() * p4.getY()) + (p3.getZ() * p4.getZ());
+	double denleft = sqrt((p1.getX() * p1.getX()) + (p1.getY() * p1.getY()) + (p1.getZ() * p1.getZ())) * sqrt((p2.getX() * p2.getX()) + (p2.getY() * p2.getY()) + (p2.getZ() * p2.getZ()));
+	double denright = sqrt((p3.getX() * p3.getX()) + (p3.getY() * p3.getY()) + (p3.getZ() * p3.getZ())) * sqrt((p4.getX() * p4.getX()) + (p4.getY() * p4.getY()) + (p4.getZ() * p4.getZ()));
+	double angleft = acos (numleft/denleft);
+	double angright = acos (numright/denright);
+	
+	
+}
+