Commit 7098cecd authored by Víctor Silos Sánchez's avatar Víctor Silos Sánchez
Browse files

Upload new file

parent e32e6a0e
#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);
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment