GroupUtils.h

Go to the documentation of this file.

#ifndef GROUP_UTILS_H_
#define GROUP_UTILS_H_
 
//#include "Transformation.h"
//#include <iostream>
 
 
 
 
//template <typename MPTraits>
//typename MPTraits::CfgType
//RotateCfgAboutBody(const std::vector<unsigned int>& _bodyList,
//                   const typename MPTraits::CfgType& _cfg,
//                   const mathtool::Orientation& _rotation,
//                   const unsigned int _dofsPerBody,
//                   const bool _debug = false) {
//  typedef typename MPTraits::CfgType CfgType;
//
//  // compute position and rotation of part A, BEFORE rotation:
//  _cfg.GetMultiBody()->Configure(_cfg);//Update transform
//  mathtool::Transformation initTA = _cfg.GetMultiBody()->
//              GetBody(_bodyList[0])->GetWorldTransformation();
//
//  const mathtool::Transformation rotation(mathtool::Vector3d(0,0,0), _rotation);
//
//  if(_debug)
//    std::cout << "Rotating bodies " << _bodyList << " with rotation = "
//              << rotation << std::endl;
//
//  // The transform to be applied to all parts (including the first one). We
//  // move the part to its relative world position with A at the world origin,
//  // then the rotation is applied, and we return the part to its relative
//  // position from A.
//  const mathtool::Transformation transform = initTA * rotation;
//
//  //Since we deal with composite CSpace, DOFs not in _bodyList should be
//  // unchanged wrt the input cfg.
//  CfgType rotatedCfg = _cfg;
//
//  //Compute each body's needed transformation and set dofs in cfg.
//  for (const unsigned int body : _bodyList) {
//    // compute position and rotation of part B
//    const mathtool::Transformation initTB = _cfg.GetMultiBody()->
//                               GetBody(body)->GetWorldTransformation();
//
//    const mathtool::Transformation TB = transform * -initTA * initTB;
//
//    //Extract the transformation:
//    std::vector<double> transformed = TB.GetCfg();
//
//    for(size_t i = 0; i < _dofsPerBody; ++i)
//      rotatedCfg[body*_dofsPerBody + i] = transformed[i];
//  }
//
//  return rotatedCfg;
//}
//
//
//
//template <typename MPTraits>
//void
//AddDofsForBodies(typename MPTraits::CfgType& _cfg,
//                 const std::vector<double>& _dofs,
//                 const std::vector<unsigned int>& _bodies) {
//  // This function adds all dofs (assumes #dofs per body is correct) to each
//  // body given in _bodies to _cfg.
//  const unsigned int dofsPerBody = _dofs.size();
//  for(const unsigned int body : _bodies)
//    for (unsigned int i = 0; i < dofsPerBody; ++i)
//      _cfg[(body * dofsPerBody) + i] += _dofs[i];
//}
//
//
//template <typename MPTraits>
//void
//OverwriteDofsFromBodies(typename MPTraits::CfgType& _toCfg,
//                        const typename MPTraits::CfgType& _fromCfg,
//                        const std::vector<unsigned int>& _bodies) {
//  // This function assigns the values from _fromCfg into _toCfg, based on which
//  // bodies are given in _bodies.
//  const unsigned int dofsPerBody = _toCfg.PosDOF() + _toCfg.OriDOF();
//  for(const unsigned int body : _bodies) {
//    const unsigned int bodyOffset = body * dofsPerBody;
//    for (unsigned int i = 0; i < dofsPerBody; ++i) {
//      const unsigned int dofIndex = bodyOffset + i;
//      _toCfg[dofIndex] = _fromCfg[dofIndex];
//    }
//  }
//}
 
#endif