<commit_before>/****************************************************************************
 * Copyright 2013 <NAME>
 * This library is distributed under the terms of the GNU Lesser General Public 
 * License (found in COPYING).
 ****************************************************************************/

using boost::shared_ptr;

/// Default constructor -- constructs a zero inertia matrix
SPATIAL_RB_INERTIA::SPATIAL_RB_INERTIA(shared_ptr<const POSE3> pose)
{
  m = (REAL) 0.0;
  h.set_zero();
  J.set_zero();
  this->pose = pose;
}

/// Default constructor -- constructs a zero inertia matrix
SPATIAL_RB_INERTIA::SPATIAL_RB_INERTIA(shared_ptr<POSE3> pose)
{
  m = (REAL) 0.0;
  h.set_zero();
  J.set_zero();
  this->pose = boost::const_pointer_cast<const POSE3>(pose);
}

/// Constructs the 6x6 spatial matrix from the given values 
SPATIAL_RB_INERTIA::SPATIAL_RB_INERTIA(REAL m, const VECTOR3& h, const MATRIX3& J, shared_ptr<const POSE3> pose)
{
  this->m = m;
  this->h = h;
  this->J = J;
  this->pose = pose;
}

/// Constructs the 6x6 spatial matrix from the given values 
SPATIAL_RB_INERTIA::SPATIAL_RB_INERTIA(REAL m, const VECTOR3& h, const MATRIX3& J, shared_ptr<POSE3> pose)
{
  this->m = m;
  this->h = h;
  this->J = J;
  this->pose = boost::const_pointer_cast<const POSE3>(pose);
}

/// Copies a spatial matrix to this one 
SPATIAL_RB_INERTIA& SPATIAL_RB_INERTIA::operator=(const SPATIAL_RB_INERTIA& m)
{
  this->pose = m.pose;
  this->m = m.m;
  this->h = m.h;
  this->J = m.J;
  return *this;
}

/// Creates a zero matrix
void SPATIAL_RB_INERTIA::set_zero()
{
  m = (REAL) 0.0;
  h.set_zero();
  J.set_zero();
}

/// Multiplies a spatial vector
void SPATIAL_RB_INERTIA::mult_spatial(const SVECTOR6& t, SVECTOR6& result) const
{
  // get necessary components of t
  ORIGIN3 ttop(t.get_upper());
  ORIGIN3 tbot(t.get_lower());

  // do some precomputation
  MATRIX3 hx = MATRIX3::skew_symmetric(h);

  // compute result
  VECTOR3 rtop(tbot*m + hx.transpose_mult(ttop), pose);
  VECTOR3 rbot(J*ttop + hx*tbot, pose);
  result.pose = pose;
  result.set_upper(rtop);
  result.set_lower(rbot);  
}

/// Multiplies a spatial vector
void SPATIAL_RB_INERTIA::mult_spatial(const SVECTOR6& t, const MATRIX3& hx, SVECTOR6& result) const
{
  // get necessary components of t
  ORIGIN3 ttop(t.get_upper());
  ORIGIN3 tbot(t.get_lower());

  // compute result
  VECTOR3 rtop(tbot*m + hx.transpose_mult(ttop), pose);
  VECTOR3 rbot(J*ttop + hx*tbot, pose);
  result.pose = pose;
  result.set_upper(rtop);
  result.set_lower(rbot);  
}

/// Multiplies the inverse of this inertia by a spatial vector
void SPATIAL_RB_INERTIA::inverse_mult_spatial(const SFORCE& w, SVECTOR6& result) const
{
  // get center-of-mass vector
  ORIGIN3 op = h/m;

  // precompute some things
  MATRIX3 opx = MATRIX3::skew_symmetric(op);
  MATRIX3 mpox = MATRIX3::skew_symmetric(-h);

  // get inertia tensor at c.o.m.
  MATRIX3 Jstar = J - opx*mpox; 

  // compute the inverse of the c.o.m. inertia matrix
  MATRIX3 iJ = MATRIX3::invert(Jstar);

  // compute opx * inv(J)
  MATRIX3 opxiJ = opx * iJ;

  // compute inverse mass
  REAL inv_m = (REAL) 1.0/m;

  // get the components of the force 
  ORIGIN3 top(w.get_upper());
  ORIGIN3 bot(w.get_lower());

  // do the arithmetic
  VECTOR3 ttop(opxiJ.transpose_mult(top) + iJ*bot, pose); 
  VECTOR3 tbot(opxiJ*opx.transpose_mult(top) + top*inv_m + opxiJ*bot, pose); 

  // set the spatial vector components
  result.pose = pose;
  result.set_upper(ttop);
  result.set_lower(tbot);
}

void SPATIAL_RB_INERTIA::inverse_mult_spatial(const SFORCE& w, const MATRIX3& iJ, const MATRIX3& opx, const MATRIX3& opxiJ, REAL inv_m, SVECTOR6& result) const
{
  // get the components of the force 
  ORIGIN3 top(w.get_upper());
  ORIGIN3 bot(w.get_lower());

  // do the arithmetic
  VECTOR3 ttop(opxiJ.transpose_mult(top) + iJ*bot, pose); 
  VECTOR3 tbot(opxiJ*opx.transpose_mult(top) + top*inv_m + opxiJ*bot, pose); 
  result.pose = pose;
  result.set_upper(ttop);
  result.set_lower(tbot);
}

/// Multiplies the inverse of this spatial matrix by a force 
SACCEL SPATIAL_RB_INERTIA::inverse_mult(const SFORCE& w) const
{
  #ifndef NEXCEPT
  if (pose != w.pose)
    throw FrameException();
  #endif

  SACCEL a;
  inverse_mult_spatial(w, a);
  return a;
}

/// Multiplies the inverse of this spatial matrix by a momentum 
SVELOCITY SPATIAL_RB_INERTIA::inverse_mult(const SMOMENTUM& w) const
{
  #ifndef NEXCEPT
  if (pose != w.pose)
    throw FrameException();
  #endif

  // get center-of-mass vector
  ORIGIN3 op = h/m;

  // precompute some things
  MATRIX3 opx = MATRIX3::skew_symmetric(op);
  MATRIX3 mpox = MATRIX3::skew_symmetric(-h);

  // get inertia tensor at c.o.m.
  MATRIX3 Jstar = J - opx*mpox; 

  // compute the inverse of the c.o.m. inertia matrix
  MATRIX3 iJ = MATRIX3::invert(Jstar);

  // compute opx * inv(J)
  MATRIX3 opxiJ = opx * iJ;

  // compute inverse mass
  REAL inv_m = (REAL) 1.0/m;

  // get the components of the momentum - this is reversed from the force version 
  ORIGIN3 top(w.get_lower());
  ORIGIN3 bot(w.get_upper());

  // do the arithmetic
  VECTOR3 ttop(opxiJ.transpose_mult(top) + iJ*bot, pose); 
  VECTOR3 tbot(opxiJ*opx.transpose_mult(top) + top*inv_m + opxiJ*bot, pose); 

  // set the spatial vector components, again reversed from the force version
  SVELOCITY result(pose);
  result.set_upper(tbot);
  result.set_lower(ttop);

  return result;
}

/// Multiplies the inverse of this spatial matrix by a force 
std::vector<SACCEL>& SPATIAL_RB_INERTIA::inverse_mult(const std::vector<SFORCE>& w, std::vector<SACCEL>& result) const
{
  result.resize(w.size());
  if (result.empty())
    return result;

  // compute inverse mass
  REAL inv_m = (REAL) 1.0/m;

  // get center-of-mass vector
  ORIGIN3 op = h/m;

  // precompute some things
  MATRIX3 opx = MATRIX3::skew_symmetric(op);
  MATRIX3 mpox = MATRIX3::skew_symmetric(-h);<commit_msg>Hopefully fixed problem of SpatialRBInertia::inverse(SMomentumd) needing to be reversed<commit_after>/****************************************************************************
 * Copyright 2013 <NAME>
 * This library is distributed under the terms of the GNU Lesser General Public 
 * License (found in COPYING).
 ****************************************************************************/

using boost::shared_ptr;

/// Default constructor -- constructs a zero inertia matrix
SPATIAL_RB_INERTIA::SPATIAL_RB_INERTIA(shared_ptr<const POSE3> pose)
{
  m = (REAL) 0.0;
  h.set_zero();
  J.set_zero();
  this->pose = pose;
}

/// Default constructor -- constructs a zero inertia matrix
SPATIAL_RB_INERTIA::SPATIAL_RB_INERTIA(shared_ptr<POSE3> pose)
{
  m = (REAL) 0.0;
  h.set_zero();
  J.set_zero();
  this->pose = boost::const_pointer_cast<const POSE3>(pose);
}

/// Constructs the 6x6 spatial matrix from the given values 
SPATIAL_RB_INERTIA::SPATIAL_RB_INERTIA(REAL m, const VECTOR3& h, const MATRIX3& J, shared_ptr<const POSE3> pose)
{
  this->m = m;
  this->h = h;
  this->J = J;
  this->pose = pose;
}

/// Constructs the 6x6 spatial matrix from the given values 
SPATIAL_RB_INERTIA::SPATIAL_RB_INERTIA(REAL m, const VECTOR3& h, const MATRIX3& J, shared_ptr<POSE3> pose)
{
  this->m = m;
  this->h = h;
  this->J = J;
  this->pose = boost::const_pointer_cast<const POSE3>(pose);
}

/// Copies a spatial matrix to this one 
SPATIAL_RB_INERTIA& SPATIAL_RB_INERTIA::operator=(const SPATIAL_RB_INERTIA& m)
{
  this->pose = m.pose;
  this->m = m.m;
  this->h = m.h;
  this->J = m.J;
  return *this;
}

/// Creates a zero matrix
void SPATIAL_RB_INERTIA::set_zero()
{
  m = (REAL) 0.0;
  h.set_zero();
  J.set_zero();
}

/// Multiplies a spatial vector
void SPATIAL_RB_INERTIA::mult_spatial(const SVECTOR6& t, SVECTOR6& result) const
{
  // get necessary components of t
  ORIGIN3 ttop(t.get_upper());
  ORIGIN3 tbot(t.get_lower());

  // do some precomputation
  MATRIX3 hx = MATRIX3::skew_symmetric(h);

  // compute result
  VECTOR3 rtop(tbot*m + hx.transpose_mult(ttop), pose);
  VECTOR3 rbot(J*ttop + hx*tbot, pose);
  result.pose = pose;
  result.set_upper(rtop);
  result.set_lower(rbot);  
}

/// Multiplies a spatial vector
void SPATIAL_RB_INERTIA::mult_spatial(const SVECTOR6& t, const MATRIX3& hx, SVECTOR6& result) const
{
  // get necessary components of t
  ORIGIN3 ttop(t.get_upper());
  ORIGIN3 tbot(t.get_lower());

  // compute result
  VECTOR3 rtop(tbot*m + hx.transpose_mult(ttop), pose);
  VECTOR3 rbot(J*ttop + hx*tbot, pose);
  result.pose = pose;
  result.set_upper(rtop);
  result.set_lower(rbot);  
}

/// Multiplies the inverse of this inertia by a spatial vector
void SPATIAL_RB_INERTIA::inverse_mult_spatial(const SFORCE& w, SVECTOR6& result) const
{
  // get center-of-mass vector
  ORIGIN3 op = h/m;

  // precompute some things
  MATRIX3 opx = MATRIX3::skew_symmetric(op);
  MATRIX3 mpox = MATRIX3::skew_symmetric(-h);

  // get inertia tensor at c.o.m.
  MATRIX3 Jstar = J - opx*mpox; 

  // compute the inverse of the c.o.m. inertia matrix
  MATRIX3 iJ = MATRIX3::invert(Jstar);

  // compute opx * inv(J)
  MATRIX3 opxiJ = opx * iJ;

  // compute inverse mass
  REAL inv_m = (REAL) 1.0/m;

  // get the components of the force 
  ORIGIN3 top(w.get_upper());
  ORIGIN3 bot(w.get_lower());

  // do the arithmetic
  VECTOR3 ttop(opxiJ.transpose_mult(top) + iJ*bot, pose); 
  VECTOR3 tbot(opxiJ*opx.transpose_mult(top) + top*inv_m + opxiJ*bot, pose); 

  // set the spatial vector components
  result.pose = pose;
  result.set_upper(ttop);
  result.set_lower(tbot);
}

void SPATIAL_RB_INERTIA::inverse_mult_spatial(const SFORCE& w, const MATRIX3& iJ, const MATRIX3& opx, const MATRIX3& opxiJ, REAL inv_m, SVECTOR6& result) const
{
  // get the components of the force 
  ORIGIN3 top(w.get_upper());
  ORIGIN3 bot(w.get_lower());

  // do the arithmetic
  VECTOR3 ttop(opxiJ.transpose_mult(top) + iJ*bot, pose); 
  VECTOR3 tbot(opxiJ*opx.transpose_mult(top) + top*inv_m + opxiJ*bot, pose); 
  result.pose = pose;
  result.set_upper(ttop);
  result.set_lower(tbot);
}

/// Multiplies the inverse of this spatial matrix by a force 
SACCEL SPATIAL_RB_INERTIA::inverse_mult(const SFORCE& w) const
{
  #ifndef NEXCEPT
  if (pose != w.pose)
    throw FrameException();
  #endif

  SACCEL a;
  inverse_mult_spatial(w, a);
  return a;
}

/// Multiplies the inverse of this spatial matrix by a momentum 
SVELOCITY SPATIAL_RB_INERTIA::inverse_mult(const SMOMENTUM& w) const
{
  #ifndef NEXCEPT
  if (pose != w.pose)
    throw FrameException();
  #endif

  // get center-of-mass vector
  ORIGIN3 op = h/m;

  // precompute some things
  MATRIX3 opx = MATRIX3::skew_symmetric(op);
  MATRIX3 mpox = MATRIX3::skew_symmetric(-h);

  // get inertia tensor at c.o.m.
  MATRIX3 Jstar = J - opx*mpox; 

  // compute the inverse of the c.o.m. inertia matrix
  MATRIX3 iJ = MATRIX3::invert(Jstar);

  // compute opx * inv(J)
  MATRIX3 opxiJ = opx * iJ;

  // compute inverse mass
  REAL inv_m = (REAL) 1.0/m;

  // get the components of the momentum
  ORIGIN3 top(w.get_upper());
  ORIGIN3 bot(w.get_lower());

  // do the arithmetic
  VECTOR3 ttop(opxiJ.transpose_mult(top) + iJ*bot, pose); 
  VECTOR3 tbot(opxiJ*opx.transpose_mult(top) + top*inv_m + opxiJ*bot, pose); 

  // set the spatial vector components
  SVELOCITY result(pose);
  result.set_lower(tbot);
  result.set_upper(ttop);

  return result;
}

/// Multiplies the inverse of this spatial matrix by a force 
std::vector<SACCEL>& SPATIAL_RB_INERTIA::inverse_mult(const std::vector<SFORCE>& w, std::vector<SACCEL>& result) const
{
  result.resize(w.size());
  if (result.empty())
    return result;

  // compute inverse mass
  REAL inv_m = (REAL) 1.0/m;

  // get center-of-mass vector
  ORIGIN3 op = h/m;

  // precompute some things
  MATRIX3 opx = MATRIX3::skew_symmetric(op);
  MATRIX3 mpox = MATRIX3::skew_symmetric(-h);
{'fraction_non_alphanumeric': 0.15060138912417415, 'fraction_numerical': 0.014399457902761308, 'mean_word_length': 3.950524109014675, 'pattern_counts': {'":': 0, '<': 23, '<?xml version=': 0, '>': 51, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4193774', 'n_tokens_mistral': 4875, 'n_tokens_neox': 4580, 'n_words': 1102}