C++ (Cpp) MATRIX3::mult Examples

Example #1

File: SpatialRBInertia.cpp Project: PositronicsLab/Ravelin

void SPATIAL_RB_INERTIA::inverse_mult_spatial(const SVECTOR6& w, const MATRIX3& iJ, const MATRIX3& hx, const MATRIX3& hxiJ, 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(hxiJ.transpose_mult(top) + iJ*bot, pose); 
  VECTOR3 tbot(top*inv_m + hx*(hxiJ.transpose_mult(top)) + hxiJ.mult(bot), pose);
  result.pose = pose;
  result.set_upper(ttop);
  result.set_lower(tbot);
}