00001 #include "../include/target_object.h" 00002 #include "../include/debug.h" 00003 #include <assert.h> 00004 #include <math.h> 00005 00006 namespace ICR 00007 { 00008 //-------------------------------------------------------------------- 00009 //-------------------------------------------------------------------- 00010 TargetObject::TargetObject() : num_cp_(0) {contact_points_.clear();} 00011 //-------------------------------------------------------------------- 00012 TargetObject::TargetObject(std::string const& name) : name_(name), num_cp_(0) {contact_points_.clear();} 00013 //-------------------------------------------------------------------- 00014 TargetObject::TargetObject(TargetObject const& src) : name_(src.name_), num_cp_(src.num_cp_), 00015 contact_points_(src.contact_points_) {} 00016 //-------------------------------------------------------------------- 00017 TargetObject& TargetObject::operator=(TargetObject const& src) 00018 { 00019 if (this !=&src) 00020 { 00021 name_=src.name_; 00022 num_cp_=src.num_cp_; 00023 contact_points_=src.contact_points_; 00024 } 00025 return *this; 00026 } 00027 //-------------------------------------------------------------------- 00028 std::ostream& operator<<(std::ostream& stream, TargetObject const& obj) 00029 { 00030 stream <<'\n'<<"TARGET OBJECT: "<<'\n' 00031 <<"Name: "<<obj.name_<<'\n' 00032 <<"Number of contact points: "<<obj.num_cp_<<'\n'<<'\n'; 00033 00034 return stream; 00035 } 00036 //-------------------------------------------------------------------- 00037 TargetObject::~TargetObject() 00038 { 00039 for (uint i=0;i<contact_points_.size();i++) 00040 delete contact_points_[i]; 00041 } 00042 //-------------------------------------------------------------------- 00043 std::string const TargetObject::getName() const {return name_;} 00044 //-------------------------------------------------------------------- 00045 void TargetObject::setName(std::string const& name) {name_=name;} 00046 //-------------------------------------------------------------------- 00047 void TargetObject::reserveCpList(uint num_cp){contact_points_.reserve(num_cp);} 00048 //-------------------------------------------------------------------- 00049 // void TargetObject::findClosestIdx(double x, double y, double z) { 00050 // for (uint i=0; i<num_cp_ ; i++) { 00051 00052 00053 // } 00054 // } 00055 // //-------------------------------------------------------------------- 00056 // void TargetObject::findClosestIdx(Eigen::Vector3d& pt) { 00057 00058 // } 00059 //-------------------------------------------------------------------- 00060 uint TargetObject::getNumCp() const {return num_cp_;} 00061 //-------------------------------------------------------------------- 00062 void TargetObject::addContactPoint(ContactPoint const& point) 00063 { 00064 //Ensure unit normal 00065 assert(fabs( (*point.getVertexNormal()).norm()-1) <= EPSILON_UNIT_NORMAL); 00066 00067 contact_points_.push_back(new ContactPoint(point)); 00068 num_cp_++; 00069 } 00070 //-------------------------------------------------------------------- 00071 void TargetObject::scaleObject(double scale) 00072 { 00073 for (uint i=0; i < contact_points_.size(); i++) 00074 *(contact_points_[i]->getVertex())=*(contact_points_[i]->getVertex())*scale; 00075 } 00076 //-------------------------------------------------------------------- 00077 ContactPoint const* TargetObject::getContactPoint(uint id) const {return contact_points_.at(id);} 00078 //-------------------------------------------------------------------- 00079 //-------------------------------------------------------------------- 00080 }//namespace ICR 00081