/**
* Returns the bounding box, which includes the plane.
*/
RotatedRectangle Plane::getMaxExpansion(){
if(!rectangleCalculated){
std::list<std::list<std::pair<Shared_Point,Shared_Point> > > edgesHull = getTransPoints();
std::vector<cv::Point2f> _points;
std::list< std::list<std::pair<Shared_Point, Shared_Point> > >::iterator it;
std::list< std::pair<Shared_Point, Shared_Point> >::iterator it_list;
for(it=edgesHull.begin(); it!=edgesHull.end(); it++){
for(it_list=(*it).begin(); it_list!=(*it).end(); it_list++){
cv::Point2f p((*it_list).first->x, (*it_list).first->z);
_points.push_back(p);
//printf("Point: %f,%f\n",p.x,p.y);
}
}
if(_points.size()==0){
fprintf(stderr,"Point size is zero so not calculating maximum expansion, edgesHull Size is: %i\n", (int)edgesHull.size());
return RotatedRectangle();
}
cv::RotatedRect rect = cv::minAreaRect(cv::Mat(_points));
double width = (rect.size.width>rect.size.height)?rect.size.width:rect.size.height;
double height = (rect.size.width<rect.size.height)?rect.size.width:rect.size.height;
double angle = rect.angle;
//Berchnung von Eckpunkten mit Hile von Trygonometrie und dann die Punkte in die Welt rotieren
Eigen::Vector3d centerOfMass(this->centerOfMass[0], this->centerOfMass[1], this->centerOfMass[2]);
Eigen::Vector3d rotationAxis = normalVector.cross(Eigen::Vector3d::UnitY());
rotationAxis.normalize();
Eigen::AngleAxisd m = Eigen::AngleAxisd(acos(normalVector[1]), rotationAxis);
Eigen::Vector3d inertiaAxisXNew = m*inertiaAxisX;
inertiaAxisXNew.normalize();
double angle2 = acos(inertiaAxisXNew.dot(Eigen::Vector3d::UnitX()));
Eigen::AngleAxisd m2;
if(angle2 != 0.0){
Eigen::Vector3d rotationAxis2 = inertiaAxisXNew.cross(Eigen::Vector3d::UnitX());
rotationAxis2.normalize();
m2 = Eigen::AngleAxisd(angle2, rotationAxis2);
}
if(angle>20){
angle-=90;
}
angle = angle/180.0*M_PI;
Eigen::Vector3d point[4];
for(int i=0; i<4; i++){
point[i].setZero();
}
/*
point[0][0] = centerOfMass[0] + (width/2.0) * cos(angle) + (height/2.0) * sin(angle);
point[0][2] = centerOfMass[2] + (width/2.0) * sin(angle) + (height/2.0) * cos(angle);
point[1][0] = centerOfMass[0] + (-width/2.0) * cos(angle) + (height/2.0) * sin(angle);
point[1][2] = centerOfMass[2] + (-width/2.0) * sin(angle) + (height/2.0) * cos(angle);
point[2][0] = centerOfMass[0] + (width/2.0) * cos(angle) + (-height/2.0) * sin(angle);
point[2][2] = centerOfMass[2] + (width/2.0) * sin(angle) + (-height/2.0) * cos(angle);
point[3][0] = centerOfMass[0] + (-width/2.0) * cos(angle) + (-height/2.0) * sin(angle);
point[3][2] = centerOfMass[2] + (-width/2.0) * sin(angle) + (-height/2.0) * cos(angle);
*/
double angle_tmp = angle;
angle=-angle;
double w = height;
double h = width;
point[0][0] = (w/2.0) * cos(angle) - (h/2.0) * sin(angle);
point[0][2] = (w/2.0) * sin(angle) + (h/2.0) * cos(angle);
point[1][0] = (-w/2.0) * cos(angle) - (h/2.0) * sin(angle);
point[1][2] = (-w/2.0) * sin(angle) + (h/2.0) * cos(angle);
point[2][0] = (w/2.0) * cos(angle) - (-h/2.0) * sin(angle);
point[2][2] = (w/2.0) * sin(angle) + (-h/2.0) * cos(angle);
point[3][0] = (-w/2.0) * cos(angle) - (-h/2.0) * sin(angle);
point[3][2] = (-w/2.0) * sin(angle) + (-h/2.0) * cos(angle);
angle = angle_tmp;
RotatedRectangle rot_rect(width, height, angle, rect.center.x, rect.center.y);
for(int i=0; i<4; i++){
point[i] += Eigen::Vector3d(rect.center.x,0,rect.center.y);
point[i] = m2.inverse() * point[i];
point[i] = m.inverse() * point[i];
point[i] = point[i] + centerOfMass;
rot_rect.edges[i]=point[i];
}
rectangle = rot_rect;
}
return rectangle;
}
/**
* Calculates a 2D hull by using alpha shapes and returns a list of pairs.
*/
std::list<std::list<std::pair<Shared_Point,Shared_Point> > > Plane::getHull(){
std::list<std::list<std::pair<Shared_Point,Shared_Point> > > edgesHull;
std::set<Shared_Point>::iterator it1;
double angle, angle2;
Eigen::Vector3d rotationAxis;
Eigen::Vector3d centerOfMass(this->centerOfMass[0], this->centerOfMass[1], this->centerOfMass[2]);
rotationAxis = normalVector.cross(Eigen::Vector3d::UnitY());
rotationAxis.normalize();
angle = acos(normalVector[1]);
Eigen::AngleAxisd m = Eigen::AngleAxisd(angle, rotationAxis);
Eigen::Vector3d inertiaAxisXNew = m*inertiaAxisX;
inertiaAxisXNew.normalize();
angle2 = acos(inertiaAxisXNew.dot(Eigen::Vector3d::UnitX()));
Eigen::AngleAxisd m2;
if(angle2 != 0.0){ //It crashed if the angle is 0.
Eigen::Vector3d rotationAxis2 = inertiaAxisXNew.cross(Eigen::Vector3d::UnitX());
rotationAxis2.normalize();
m2 = Eigen::AngleAxisd(angle2, rotationAxis2);
}
edgesHull = getTransPoints();
if(edgesHull.size()==0)
return edgesHull;
Shared_Point p = edgesHull.begin()->begin()->first;
Shared_Point p2= edgesHull.begin()->begin()->first;
std::list< std::list<std::pair<Shared_Point, Shared_Point> > >::iterator it;
std::list< std::pair<Shared_Point, Shared_Point> >::iterator it_list;
for(it=edgesHull.begin(); it!=edgesHull.end(); it++){
for(it_list=(*it).begin(); it_list!=(*it).end(); it_list++){
if(fabs((*it_list).first->x)>fabs(p->x))
p=(*it_list).first;
if(fabs((*it_list).first->z)>fabs(p2->z))
p2=(*it_list).first;
if(fabs((*it_list).second->x)>fabs(p->x))
p=(*it_list).second;
if(fabs((*it_list).second->z)>fabs(p2->z))
p2=(*it_list).second;
}
}
std::list< std::list< std::pair<Shared_Point, Shared_Point> > >::iterator it2;
std::list< std::pair<Shared_Point, Shared_Point> >::iterator it3;
for(it2=edgesHull.begin(); it2!=edgesHull.end(); it2++){
for(it3=(*it2).begin(); it3!=(*it2).end(); it3++){
Eigen::Vector3d p((*it3).first->p[0], (*it3).first->p[1], (*it3).first->p[2]);
Eigen::Vector3d p1((*it3).second->p[0], (*it3).second->p[1], (*it3).second->p[2]);
p = m2.inverse() * p;
p = m.inverse() * p;
p = p + centerOfMass;
(*it3).first->p[0]=p[0];
(*it3).first->p[1]=p[1];
(*it3).first->p[2]=p[2];
p1 = m2.inverse() * p1;
p1 = m.inverse() * p1;
p1 = p1 + centerOfMass;
(*it3).second->p[0]=p1[0];
(*it3).second->p[1]=p1[1];
(*it3).second->p[2]=p1[2];
}
}
return edgesHull;
}
