/**
* Recursion method to be used with traverseTreeTopDown() and recursion parameters
* of type *Vector3RecursionParams*.
*
* Re-arranges the joint-transform of the recursion link's *parent joint*, along with
* the link's visual/collision/intertial rotations, such that all joints rotate around the axis
* given in the recursion parameters vector.
*/
int allRotationsToAxisCB(urdf_traverser::RecursionParamsPtr& p)
{
urdf_traverser::LinkPtr link = p->getLink();
if (!link)
{
ROS_ERROR("allRotationsToAxis: NULL link passed");
return -1;
}
Vector3RecursionParams::Ptr param = baselib_binding_ns::dynamic_pointer_cast<Vector3RecursionParams>(p);
if (!param)
{
ROS_ERROR("Wrong recursion parameter type");
return -1;
}
urdf_traverser::JointPtr joint = link->parent_joint;
if (!joint)
{
ROS_INFO_STREAM("allRotationsToAxis: Joint for link " << link->name << " is NULL, so this must be the root joint");
return 1;
}
Eigen::Vector3d axis = param->vec;
Eigen::Quaterniond alignAxis;
if (urdf_traverser::jointTransformForAxis(joint, axis, alignAxis))
{
Eigen::Vector3d rotAxis(joint->axis.x, joint->axis.y, joint->axis.z);
// ROS_INFO_STREAM("Transforming axis "<<rotAxis<<" for joint "<<joint->name<<" with transform "<<urdf_traverser::EigenTransform(alignAxis));
urdf_traverser::applyTransform(joint, urdf_traverser::EigenTransform(alignAxis), false);
// the link has to receive the inverse transform, so it stays at the original position
Eigen::Quaterniond alignAxisInv = alignAxis.inverse();
urdf_traverser::applyTransform(link, urdf_traverser::EigenTransform(alignAxisInv), true);
// we also have to fix the child joint's (1st order child joints) transform
// to correct for this transformation.
for (std::vector<urdf_traverser::JointPtr>::iterator pj = link->child_joints.begin();
pj != link->child_joints.end(); pj++)
{
urdf_traverser::applyTransform(*pj, urdf_traverser::EigenTransform(alignAxisInv), true);
}
// finally, set the rotation axis to the target
joint->axis.x = axis.x();
joint->axis.y = axis.y();
joint->axis.z = axis.z();
}
// all good, indicate that recursion can continue
return 1;
}
void DeformToFit::registerAndDeformNodes(Structure::Node * snode, Structure::Node * tnode)
{
auto scenter = snode->position(Eigen::Vector4d(0.5, 0.5, 0, 0));
auto tcenter = tnode->position(Eigen::Vector4d(0.5, 0.5, 0, 0));
auto translation = Vector3(tcenter - scenter);
if (snode->type() == tnode->type())
{
if (snode->type() == Structure::CURVE)
{
auto scpts = snode->controlPoints();
auto tcpts = tnode->controlPoints();
for (auto & p : tcpts) p -= translation;
// Register
Vector3 sfront = scpts.front();
Vector3 tfront = tcpts.front();
Vector3 tback = tcpts.back();
bool isReverse = (sfront - tfront).norm() > (sfront - tback).norm() ? true : false;
if (isReverse) std::reverse(tcpts.begin(), tcpts.end());
// Encode target curve as deltas from center
std::map < double, Vector3 > deltas;
for (size_t i = 0; i < tcpts.size(); i++){
double t = double(i) / (tcpts.size() - 1);
deltas[t] = tcpts[i] - scenter;
}
// Deform source curve from deltas
for (size_t i = 0; i < scpts.size(); i++){
double t = double(i) / (scpts.size() - 1);
scpts[i] = (scenter + translation) + linear_interpolate<Vector3>(t, deltas);
}
// Apply deformed control points
snode->setControlPoints(scpts);
}
if (snode->type() == Structure::SHEET)
{
Structure::Sheet * ssheet = (Structure::Sheet*) snode;
Structure::Sheet * tsheet = (Structure::Sheet*) tnode;
auto ssurface = ssheet->surface;
auto tsurface = tsheet->surface;
// Remove translation
tsurface.translate(-translation);
// Minimize rotation
Vector3 tpos, tu, tv, tnormal;
tsurface.GetFrame(0.5, 0.5, tpos, tu, tv, tnormal);
Vector3 spos, su, sv, snormal;
ssurface.GetFrame(0.5, 0.5, spos, su, sv, snormal);
if (snormal.dot(tnormal) < 0)
tnormal *= -1;
Eigen::Quaterniond q = Eigen::Quaterniond::FromTwoVectors(snormal, tnormal);
for (auto & row : tsurface.mCtrlPoint) for (auto & p : row) p = (q.inverse() * (p - scenter)) + scenter;
QMap < double, QVector<double> > dists;
for (double u = 0; u <= 1.0; u += 1.0){
for (double v = 0; v <= 1.0; v += 1.0){
for (double i = 0; i <= 1.0; i += 1.0){
for (double j = 0; j <= 1.0; j += 1.0){
dists[(ssurface.P(u, v) - tsurface.P(i, j)).norm()] = (QVector<double>() << u << v << i << j);
}
}
}
}
auto bestChoice = dists.values().front();
bool isReverseU = bestChoice[0] != bestChoice[2], isReverseV = bestChoice[1] != bestChoice[3];
// Reverse if needed
if ( isReverseV ){
for (int i = 0; i < (int)tsurface.mCtrlPoint.size(); i++){
std::reverse(tsurface.mCtrlPoint[i].begin(), tsurface.mCtrlPoint[i].end());
std::reverse(tsurface.mCtrlWeight[i].begin(), tsurface.mCtrlWeight[i].end());
}
}
if( isReverseU ){
std::reverse(tsurface.mCtrlPoint.begin(), tsurface.mCtrlPoint.end());
std::reverse(tsurface.mCtrlWeight.begin(), tsurface.mCtrlWeight.end());
}
std::map < double, size_t > mapU, mapV;
for (size_t i = 0; i < tsurface.mNumUCtrlPoints; i++) mapU[double(i) / (tsurface.mNumUCtrlPoints - 1)] = i;
for (size_t j = 0; j < tsurface.mNumVCtrlPoints; j++) mapV[double(j) / (tsurface.mNumVCtrlPoints - 1)] = j;
auto getQuad = [&](size_t u, size_t v, Array2D_Vector3& cpts){
return QVector<Vector3>() << cpts[u][v] << cpts[u+1][v] << cpts[u][v+1] << cpts[u+1][v+1];
};
for (size_t i = 0; i < ssurface.mNumUCtrlPoints; i++){
for (size_t j = 0; j < ssurface.mNumVCtrlPoints; j++){
double u = double(i) / (ssurface.mNumUCtrlPoints-1);
double v = double(j) / (ssurface.mNumVCtrlPoints-1);
auto weight_u = linear_interpolate_weight<size_t>(u, mapU);
auto weight_v = linear_interpolate_weight<size_t>(v, mapV);
std::pair <size_t, size_t> quad_idx(std::get<0>(weight_u), std::get<0>(weight_v));
std::pair <double, double> quad_uv(std::get<2>(weight_u), std::get<2>(weight_v));
auto quad = getQuad(quad_idx.first, quad_idx.second, tsurface.mCtrlPoint);
auto interp = quad_interpolate(quad[0], quad[1], quad[2], quad[3], quad_uv.first, quad_uv.second);
ssurface.mCtrlPoint[i][j] = interp;
}
}
// Apply rotation
for (auto & row : ssurface.mCtrlPoint) for (auto & p : row) p = (q * (p - scenter)) + tcenter;
ssheet->surface.mCtrlPoint = ssurface.mCtrlPoint;
ssheet->surface.quads.clear();
}
}
else
{
Structure::Curve curve((snode->type() == Structure::CURVE) ? (*(Structure::Curve*)snode) : (*(Structure::Curve*)tnode));
Structure::Sheet sheet((snode->type() == Structure::SHEET) ? (*(Structure::Sheet*)snode) : (*(Structure::Sheet*)tnode));
double minU = std::min((sheet.surface.P(0, 0) - sheet.surface.P(1, 0)).norm(), (sheet.surface.P(0, 1) - sheet.surface.P(1, 1)).norm());
double minV = std::min((sheet.surface.P(0, 0) - sheet.surface.P(0, 1)).norm(), (sheet.surface.P(1, 0) - sheet.surface.P(1, 1)).norm());
bool isProjectAlongU = minU < minV;
// Roll up sheet
int idx = (isProjectAlongU) ? (sheet.surface.mNumUCtrlPoints - 1) * 0.5 : (sheet.surface.mNumVCtrlPoints - 1) * 0.5;
Array1D_Vector3 projection = (isProjectAlongU) ? sheet.surface.GetControlPointsV(idx) : sheet.surface.GetControlPointsU(idx);
Array2D_Vector3 ctrlPnts(sheet.surface.mNumUCtrlPoints);
if (isProjectAlongU){
ctrlPnts = Array2D_Vector3(sheet.surface.mNumUCtrlPoints, projection);
}else{
for (size_t i = 0; i < sheet.surface.mNumUCtrlPoints; i++)
ctrlPnts[i] = Array1D_Vector3(sheet.surface.mNumVCtrlPoints, projection[i]);
}
sheet.surface.mCtrlPoint = ctrlPnts;
Structure::Curve curveFromSheet(NURBS::NURBSCurved::createCurveFromPoints(isProjectAlongU ?
sheet.surface.GetControlPointsV(0) : sheet.surface.GetControlPointsU(0)), "temp");
// Sheet to curve case:
if (snode->type() == Structure::SHEET)
{
DeformToFit::registerAndDeformNodes(&curveFromSheet, &curve);
if (isProjectAlongU)
sheet.surface.mCtrlPoint = Array2D_Vector3(sheet.surface.mNumUCtrlPoints, curveFromSheet.curve.mCtrlPoint);
else{
for (size_t i = 0; i < sheet.surface.mNumUCtrlPoints; i++)
sheet.surface.mCtrlPoint[i] = Array1D_Vector3(sheet.surface.mNumVCtrlPoints, curveFromSheet.curve.mCtrlPoint[i]);
}
((Structure::Sheet*)snode)->surface.mCtrlPoint = sheet.surface.mCtrlPoint;
}
// Curve to sheet case:
if (snode->type() == Structure::CURVE)
{
DeformToFit::registerAndDeformNodes(snode, &curveFromSheet);
}
}
}
/**
* @brief Rotate the map relative to its local frame
*
* @param rotation <a href="http://eigen.tuxfamily.org/dox/group__Geometry__Module.html#ga0d2bd45f1215359f8e7c0d7ab53c4acb" target="_blank">
* Eigen::Quaterniond</a>
*/
void rotate(const Eigen::Quaterniond &rotation)
{
data_ptr->offset.rotate(rotation.inverse());
}
Eigen::Quaterniond UAS::transform_frame(const Eigen::Quaterniond &q)
{
return FRAME_ROTATE_Q * q * FRAME_ROTATE_Q.inverse();
}