Rectangle::Rectangle(PointPtr center, double width, double height) : center(center), width(width), height(height) {
points.insert(points.end(), PointPtr(new Point(center->x - width / 2, center->y + height / 2)));
points.insert(points.end(), PointPtr(new Point(center->x + width / 2, center->y + height / 2)));
points.insert(points.end(), PointPtr(new Point(center->x + width / 2, center->y - height / 2)));
points.insert(points.end(), PointPtr(new Point(center->x - width / 2, center->y - height / 2)));
build();
}
void FullSpiralStc::initialize(PointPtr starting_point, double tool_size) {
this->tool_size = tool_size;
// Initialize starting_cell
starting_cell = PartiallyOccupiableCellPtr(new PartiallyOccupiableCell(
PointPtr(new Point(starting_point->x - tool_size / 2, starting_point->y + tool_size / 2)), 2 * tool_size));
starting_cell->set_parent(PartiallyOccupiableCellPtr(new PartiallyOccupiableCell(
PointPtr(new Point(starting_cell->get_center()->x, starting_cell->get_center()->y - 2 * tool_size)),
2 * tool_size)));
path.insert(path.end(), starting_point);
}
void MstcOnline::initialize(PointPtr starting_point, double tool_size, CommunicatorPtr communicator) {
communicator->set_tool_size(tool_size);
this->tool_size = tool_size;
this->communicator = communicator;
// Initialize starting_cell
starting_cell = IdentifiableCellPtr(
new IdentifiableCell(PointPtr(new Point(starting_point->x - tool_size / 2, starting_point->y + tool_size / 2)),
2 * tool_size, communicator->get_robot_name()));
starting_cell->set_parent(IdentifiableCellPtr(new IdentifiableCell(
PointPtr(new Point(starting_cell->get_center()->x, starting_cell->get_center()->y - 2 * tool_size)),
2 * tool_size, communicator->get_robot_name())));
path.insert(path.end(), starting_point);
}
Template::Template(std::string name, PointVector points) : Name(name), references(0)
{
for (size_t i = 0; i < points.size(); ++i)
{
Points.push_back(PointPtr(new Point(points[i]->X,points[i]->Y)));
}
Resample(Points, NumPoints);
RotateToZero(Points);
ScaleToSquare(Points, SquareSize);
TranslateToOrigin(Points);
}
void TranslateToOrigin(PointVector& points)
{
PointPtr c = Centroid(points);
PointVector newpoints;
for (size_t i = 0; i < points.size(); ++i)
{
double qx = points[i]->X - c->X;
double qy = points[i]->Y - c->Y;
newpoints.push_back(PointPtr(new Point(qx, qy)));
}
points.clear();
points.insert(points.begin(), newpoints.begin(), newpoints.end());
}
void ScaleToSquare(PointVector& points, double size)
{
RectanglePtr B = BoundingBox(points);
PointVector newpoints;
for (size_t i = 0; i < points.size(); ++i)
{
double qx = points[i]->X * (size / B->Width);
double qy = points[i]->Y * (size / B->Height);
newpoints.push_back(PointPtr(new Point(qx, qy)));
}
points.clear();
points.insert(points.begin(), newpoints.begin(), newpoints.end());
}
PointPtr Centroid(PointVector& points)
{
double x = 0;
double y = 0;
for (size_t i = 0; i < points.size(); ++i)
{
x += points[i]->X;
y += points[i]->Y;
}
x /= points.size();
y /= points.size();
return PointPtr(new Point(x, y));
}
ReaderImpl::ReaderImpl(std::istream& ifs)
: m_ifs(ifs)
, m_size(0)
, m_current(0)
// , m_point_reader(PointReaderPtr())
, m_header_reader(new reader::Header(m_ifs))
, m_header(HeaderPtr())
, m_point(PointPtr(new liblas::Point()))
, m_filters(0)
, m_transforms(0)
, bNeedHeaderCheck(false)
{
}
void RotateBy(PointVector& points, double theta)
{
PointPtr c = Centroid(points);
double cosine = cos(theta);
double sine = sin(theta);
PointVector newpoints;
for (size_t i = 0; i < points.size(); ++i)
{
double qx = (points[i]->X - c->X) * cosine - (points[i]->Y - c->Y) * sine + c->X;
double qy = (points[i]->X - c->X) * sine + (points[i]->Y - c->Y) * cosine + c->Y;
newpoints.push_back(PointPtr(new Point(qx, qy)));
}
points.clear();
points.insert(points.begin(), newpoints.begin(), newpoints.end());
}
Pose(Point *p, Quaternion* q) {
this->point = PointPtr(p);
this->quat = QuaternionPtr(q);
};
Pose(const Pose& other) {
this->point = PointPtr(new Point(*(other.point)));
this->quat = QuaternionPtr(new Quaternion(*(other.quat)));
};
Pose() {
this->point = PointPtr(new Point(0, 0, 0));
this->quat = QuaternionPtr(new Quaternion(0, 0, 0, 0));
};
