00001 // -*- mode: c++; indent-tabs-mode: nil; c-basic-offset: 4; tab-width: 4; -*- 00002 #include "ConfigurationSpace.h" 00003 #include "PathPlanner.h" 00004 #include "OmniWheel.h" 00005 #define _USE_MATH_DEFINES // for MSVC 00006 #include <math.h> 00007 00008 using namespace PathEngine; 00009 00010 inline double theta_limit(double theta) 00011 { 00012 while (theta >= 2*M_PI) theta -= 2*M_PI; 00013 while (theta < 0) theta += 2*M_PI; 00014 return theta; 00015 } 00016 00017 Configuration OmniWheel::interpolate(const Configuration& from, 00018 const Configuration& to, 00019 double ratio) const 00020 { 00021 ConfigurationSpace *cspace = planner_->getConfigurationSpace(); 00022 Configuration cfg(cspace->size()); 00023 for (unsigned int i=0; i<cfg.size(); i++){ 00024 if (cspace->unboundedRotation(i)){ 00025 double dth = to.value(i) - from.value(i); 00026 dth = theta_limit(dth); 00027 if (dth > M_PI){ 00028 dth = dth - 2*M_PI; 00029 } 00030 cfg.value(i) = from.value(i) + ratio*dth; 00031 }else{ 00032 cfg.value(i) = (1-ratio)*from.value(i) + ratio*to.value(i); 00033 } 00034 } 00035 00036 return cfg; 00037 } 00038 00039 double OmniWheel::distance(const Configuration& from, const Configuration& to) const 00040 { 00041 ConfigurationSpace *cspace = planner_->getConfigurationSpace(); 00042 double v=0, d; 00043 for (unsigned int i=0; i<cspace->size(); i++){ 00044 if (cspace->unboundedRotation(i)){ 00045 double dth = theta_limit(to.value(i) - from.value(i)); 00046 if (dth > M_PI) dth = 2*M_PI - dth; 00047 d = cspace->weight(i)*dth; 00048 }else{ 00049 d = cspace->weight(i)*(to.value(i) - from.value(i)); 00050 } 00051 v += d*d; 00052 } 00053 return sqrt(v); 00054 } 00055 00056 00057