C++ (Cpp) MATRIX3::get_row Examples

Example #1

File: FixedJoint.cpp Project: PositronicsLab/no-slip-and-viscous-experiments

/// Setup the transform from one joint to another
void FIXEDJOINT::setup_joint()
{
  const unsigned X = 0, Y = 1, Z = 2;
  shared_ptr<const POSE3> GLOBAL;

  // get the two poses 
  shared_ptr<const POSE3> inboard = get_inboard_pose();
  shared_ptr<const POSE3> outboard = get_outboard_pose();
  if (!inboard || !outboard)
    return;

  // get the rotation matrices
  MATRIX3 Ri = inboard->q;
  MATRIX3 Ro = outboard->q;

  // TODO: fix this
  // compute the relative transform
/*
  MATRIX3 Rrel = MATRIX3::transpose(Ro) * Ri;
  _T->q = Rrel;
  _T->x.set_zero();
  _T->source = ;

  // compute the vector from the inner link to the outer link in inner link
  // frame
  VECTOR3 ox(outboard->x, To);
//  _ui = inboard->inverse_transform(ox);
*/
  // compute the constant orientation term
  _rconst[X] = VECTOR3(Ri.get_row(X), GLOBAL).dot(VECTOR3(Ro.get_row(X), GLOBAL));
  _rconst[Y] = VECTOR3(Ri.get_row(Y), GLOBAL).dot(VECTOR3(Ro.get_row(Y), GLOBAL));
  _rconst[Z] = VECTOR3(Ri.get_row(Z), GLOBAL).dot(VECTOR3(Ro.get_row(Z), GLOBAL));
}