/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/matvecIO.cpp

Go to the documentation of this file.

//######################################################################
//
// GraspIt!
// Copyright (C) 2002-2009  Columbia University in the City of New York.
// All rights reserved.
//
// GraspIt! is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// GraspIt! is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
//
// Author(s): Andrew T. Miller 
//
// $Id: matvecIO.cpp,v 1.5 2009/03/25 22:10:04 cmatei Exp $
//
//######################################################################

#include "matvecIO.h"
#include <qstringlist.h>
//Added by qt3to4:
#include <QTextStream>
#include "mytools.h"

QTextStream&
operator>>(QTextStream &is, vec3 &v)
{
  QChar ch;
  double x,y,z;
  is >> ch >> x >> y >> z >> ch;
  v[0] = x; v[1] = y; v[2] = z;
  
  return is;
}

QTextStream &
operator<<(QTextStream &os, const vec3 &v)
{
  int oldFlags = os.setf(QTextStream::showpos);
  os << '[' << v[0] << ' ' << v[1] << ' ' << v[2] << ']';
  os.flags(oldFlags);
  return os;
}

QTextStream&
operator>>(QTextStream &is, position &p)
{
  QChar ch;
  double x,y,z;
  is >> ch >> x >> y >> z >> ch;
  p[0] = x; p[1] = y; p[2] = z;

  return is;
}

QTextStream &
operator<<(QTextStream &os, const position &p)
{
  int oldFlags = os.setf(QTextStream::showpos);
  os << '[' << p[0] << ' ' << p[1] << ' ' << p[2] << ']';
  os.flags(oldFlags);
  return os;
}

QTextStream &
operator>>(QTextStream &is, mat3 &m)
{
  is >> m[0] >> m[3] >> m[6];
  is >> m[1] >> m[4] >> m[7];
  is >> m[2] >> m[5] >> m[8];
  return is;
}

QTextStream &
operator<<(QTextStream &os, const mat3 &m)
{
  int oldFlags = os.setf(QTextStream::showpos);
  os << '[' << m[0] << ' ' << m[3] << ' ' << m[6] << ']' << endl;
  os << '[' << m[1] << ' ' << m[4] << ' ' << m[7] << ']' << endl;
  os << '[' << m[2] << ' ' << m[5] << ' ' << m[8] << ']' << endl;
  os.flags(oldFlags);
  return os;
}

QTextStream &
operator>>(QTextStream &is, Quaternion &q)
{
  QChar ch;
  double w,x,y,z;
  is >> ch >> w >> x >> y >> z >> ch;
  q.w = w; q.x = x; q.y = y; q.z = z;
  return is;
}

QTextStream&
operator<<(QTextStream &os, const Quaternion &q)
{
  int oldFlags = os.setf(QTextStream::showpos);
  os <<'('<< q.w <<' '<< q.x <<' '<< q.y <<' '<< q.z <<')';
  os.flags(oldFlags);
  return os;
}

QTextStream&
operator>>(QTextStream &is, transf &tr)
{
  Quaternion q;
  vec3 v;

  is >> q >> v;
  tr.set(q,v);

  return is;
}

QTextStream &
operator<<(QTextStream &os, const transf &tr)
{
  return os << tr.rotation() << tr.translation();
}

int
readTransRotFromQTextStream(QTextStream &stream,transf &tr)
{
  QString line;
  QChar axis;
  double theta,x,y,z;
  transf tmpTr;
  vec3 tmpVec;
  bool ok;
  QStringList strings;
  
  tr = transf::IDENTITY;

        while (1) {
                line = stream.readLine();
                if ( line.isNull() ) break;
                if ( !line.isEmpty() && line[0]!='#' ) break;
        }

  do{
        if ( !line.isNull() &&(line[0]=='r' || line[0]=='t' || line[0]=='T' || line[0]=='R')){
      if (line[0]=='r') {  /* rotation */
                strings = QStringList::split(QChar(' '),line);
                if (strings.count() < 3) return FAILURE;
                theta = strings[1].toDouble(&ok); if (!ok) return FAILURE;
                axis = strings[2][0];

                if (strings[0]!="rr") /* radians */
                  theta *= M_PI/180.0;
        
                if (axis == 'x') tmpVec = vec3::X;
                else if (axis == 'y') tmpVec = vec3::Y;
                else if (axis == 'z') tmpVec = vec3::Z;
                else return FAILURE;

                tmpTr = rotate_transf(theta,tmpVec);
      }
      else if (line[0]=='t') { /* translation */
                strings = QStringList::split(QChar(' '),line);
                if (strings.count() < 4) return FAILURE;
                x = strings[1].toDouble(&ok); if (!ok) return FAILURE;
                y = strings[2].toDouble(&ok); if (!ok) return FAILURE;
                z = strings[3].toDouble(&ok); if (!ok) return FAILURE;
                tmpVec = vec3(x,y,z);
                //      printf("   Reading translation transform...\n");
                tmpTr = translate_transf(tmpVec);
      }
      else if (line[0]=='T') { /* full transform: (Quaternion)[Translation] */
                QString trStr = line.section(' ',1,-1);
                QTextStream lineStream(&trStr,QIODevice::ReadOnly);
                lineStream >> tmpTr;  
      }
      else if (line[0]=='R') { /* Rotation matrix */
                mat3 tmpMat;
                stream >> tmpMat;               
                tmpTr = transf(tmpMat,vec3::ZERO);
                line=stream.readLine();
      }
      tr = tmpTr * tr;
    }
    line=stream.readLine();
  }while ( !line.isNull() && (line[0]=='r' || line[0]=='t' || line[0]=='T' || line[0]=='R'));

  return SUCCESS;
}

---

graspit
Author(s):
autogenerated on Wed Jan 25 10:58:53 2012