bool PairRelationDetector::has_trans_relation(int id1, int id2)
{
Eigen::Vector3d transVec = nodesCenter_[id1] - nodesCenter_[id2];
Eigen::Matrix4d transMat = create_translation3d(transVec);
Eigen::MatrixXd newverts2 = transform_point3d(nodesCpts_[id2], transMat);
double min_dist, mean_dist, max_dist;
distanceBetween(nodesCpts_[id1], newverts2, min_dist, mean_dist, max_dist);
double error = 2 * mean_dist / (nodesDiameter_[id1] + nodesDiameter_[id2]);
double nerror = fixDeviationByPartName(graph_->nodes[id1]->id, graph_->nodes[id2]->id, error);
if (nerror < thTransRadio_)
{
PairRelation pr(graph_->nodes[id1], graph_->nodes[id2]);
pr.trans_vec = transVec;
pr.diameter = computePairDiameter(pr);
pr.deviation = error;
pr.tag = true;
transPairs_.push_back(pr);
return true;
}
else
return false;
}
void rotate_points3d(const Eigen::MatrixXd& ptsin, const Point_3& center, const Vector_3& direction, double angle, Eigen::MatrixXd& ptsout)
{
Eigen::Vector3d c(center.x(), center.y(), center.z());
Eigen::Vector3d dir(direction.x(), direction.y(), direction.z());
Eigen::Matrix4d rotMat = create_rotation3d_line_angle(center, direction, angle);
ptsout.resize(ptsin.rows(), ptsin.cols() );
ptsout = transform_point3d(ptsin, rotMat);
}
double PairRelationDetector::TransPairModifier::operator() (Structure::Node *n1, Eigen::MatrixXd& m1, Structure::Node *n2, Eigen::MatrixXd& m2)
{
double deviation(-1.0), min_dist, mean_dist, max_dist;
if ( n1->id == n2->id)
return deviation;
Eigen::Vector3d transVec = n1->center() - n2->center();
Eigen::Matrix4d transMat = create_translation3d(transVec);
Eigen::MatrixXd newverts2 = transform_point3d(m2, transMat);
distanceBetween(m1, newverts2, min_dist, mean_dist, max_dist);
deviation = 2 * mean_dist / (n1->bbox().diagonal().norm() + n2->bbox().diagonal().norm());
if ( deviation > maxAllowDeviation_)
{
maxAllowDeviation_ = deviation;
}
return deviation;
}