kernel/Joint.h

Go to the documentation of this file.

#ifndef JOINT_H
#define JOINT_H

#include <iostream>
#include <string>
#include <vector>
#include <stdlib.h>
#include <stdio.h>

#include "Point.h"
#include "DH_Link.h"

template <class DataType, int dim>
class DH_Link;

template <class DataType, int dim>
class DH_Chain;

template <class DataType, int dim>
class SceneBase;

template <class DataType, int dim>
class Scene;

template <class DataType, int dim>
class Joint {
public:
  friend class DH_Link<DataType,dim>;
  friend class DH_Chain<DataType,dim>;
  friend class SceneBase<DataType,dim>;
  friend class Scene<DataType,dim>;
  enum actuationType { revolute, prismatic, sphericalWrist };

  // Constructors
  Joint() {
    setDefault();
  }

  Joint( const Joint& v) {
    CFidTag = v.CFidTag;
    origin = v.origin;
    xaxis = v.xaxis;
    yaxis = v.yaxis;
    zaxis = v.zaxis;
    type = v.type;
    connectionId = v.connectionId;
    connectionOrientation = v.connectionOrientation;
  }

  Joint( PType o, PType z, PType x, int t ) {
    setDefault();
    origin = o;
    zaxis = normalize(z);
    xaxis = normalize(x);
    yaxis = cross3D(zaxis,xaxis);
    yaxis = normalize(yaxis);
    type = t;
#ifdef JOINT_DEBUG_ON
    ( mklog() << "joint := origin " << origin << " zaxis " << zaxis << " xaxis " << xaxis
              << " yaxis " << yaxis << " type " << type << std::endl ).flush();
#endif
  }

  Joint( PType o, PType z, PType x, int t, std::vector<int> cf ) {
    //Joint(o,z,x,t);
    setDefault();
    origin = o;
    zaxis = normalize(z);
    xaxis = normalize(x);
    yaxis = cross3D(zaxis,xaxis);
    yaxis = normalize(yaxis);
    type = t;
#ifdef JOINT_DEBUG_ON
    ( mklog() << "joint := origin " << origin << " zaxis " << zaxis << " xaxis " << xaxis
              << " yaxis " << yaxis << " type " << type << std::endl ).flush();
#endif
    CFidTag = cf;
  }

  // Destructor
  ~Joint() {}

  void setDefault() {
    id = -1;
    CFidTag.clear();
    origin(0) = 1;  origin(1) = 0.;  origin(2) = 0.;
    xaxis(0) = 1;  xaxis(1) = 0.;  xaxis(2) = 0.;
    yaxis(0) = 0;  yaxis(1) = 1.;  yaxis(2) = 0.;
    zaxis(0) = 0;  zaxis(1) = 0.;  zaxis(2) = 1.;

    type = -1;
    connectionId = -1;
    connectionOrientation = -1;
  }

  virtual std::vector<int>& CFIdTag() { return CFidTag; }

  void setId(int v) { id = v; }

  virtual void Restart(std::ifstream& ifs, int& pos, double& t, double& dt) {
    ifs.seekg(pos);
#ifdef CHECKRESTART_DEBUG_ON
    ( mklog() << "\nREstarting Joint\n" ).flush();
#endif

    ifs.read((char*)&origin(0),sizeof(DataType));
    ifs.read((char*)&origin(1),sizeof(DataType));
    ifs.read((char*)&origin(2),sizeof(DataType));
    ifs.read((char*)&xaxis(0),sizeof(DataType));
    ifs.read((char*)&xaxis(1),sizeof(DataType));
    ifs.read((char*)&xaxis(2),sizeof(DataType));
    ifs.read((char*)&yaxis(0),sizeof(DataType));
    ifs.read((char*)&yaxis(1),sizeof(DataType));
    ifs.read((char*)&yaxis(2),sizeof(DataType));
    ifs.read((char*)&zaxis(0),sizeof(DataType));
    ifs.read((char*)&zaxis(1),sizeof(DataType));
    ifs.read((char*)&zaxis(2),sizeof(DataType));
    ifs.read((char*)&type,sizeof(int));
    ifs.read((char*)&connectionId,sizeof(int));
    ifs.read((char*)&connectionOrientation,sizeof(int));
#ifdef CHECKRESTART_DEBUG_ON
    ( mklog() << " origin " << origin << " xaxis " << xaxis << " yaxis " << yaxis
              << " zaxis " << zaxis << " type " << type << " connectionId " << connectionId
              << " connectionOrientaiton " << connectionOrientation << std::endl ).flush();
#endif
    pos = ifs.tellg();
  }

  virtual void Checkpointing(std::ofstream& ofs) {
#ifdef CHECKRESTART_DEBUG_ON
    ( mklog() << "\nCHECKPOINTING Joint " << id << std::endl ).flush();
#endif
    ofs.write((char*)&origin(0),sizeof(DataType));
    ofs.write((char*)&origin(1),sizeof(DataType));
    ofs.write((char*)&origin(2),sizeof(DataType));
    ofs.write((char*)&xaxis(0),sizeof(DataType));
    ofs.write((char*)&xaxis(1),sizeof(DataType));
    ofs.write((char*)&xaxis(2),sizeof(DataType));
    ofs.write((char*)&yaxis(0),sizeof(DataType));
    ofs.write((char*)&yaxis(1),sizeof(DataType));
    ofs.write((char*)&yaxis(2),sizeof(DataType));
    ofs.write((char*)&zaxis(0),sizeof(DataType));
    ofs.write((char*)&zaxis(1),sizeof(DataType));
    ofs.write((char*)&zaxis(2),sizeof(DataType));
    ofs.write((char*)&type,sizeof(int));
    ofs.write((char*)&connectionId,sizeof(int));
    ofs.write((char*)&connectionOrientation,sizeof(int));
#ifdef CHECKRESTART_DEBUG_ON
    ( mklog() << " origin " << origin << " xaxis " << xaxis << " yaxis " << yaxis
              << " zaxis " << zaxis << " type " << type << " connectionId " << connectionId
              << " connectionOrientaiton " << connectionOrientation << std::endl ).flush();
#endif
  }

  virtual PType getOrigin() { return origin; }

  virtual void setOrigin(PType& o) {
    ( mklog() << " Changing joint origin to " << o  ).flush();
    origin = o;
    ( mklog() << " origin = " << origin << std::endl ).flush();
  }

protected:
  int id; 
  std::vector<int> CFidTag; 
  PType origin, xaxis, yaxis, zaxis;
  int type, connectionId, connectionOrientation;
  int CFidTag_size;
};

#endif // JOINT_H

---