text: #include "stdafx.h"
  
  #include "ik_dbg_matrix.h"
  
  #include "ik/iklimb.h"
  
  u32 sdbg_state_sequence_number = 130;
  
  IC Fmatrix& cvm(Matrix& IM) { return *((Fmatrix*)(&IM)); }
  
  #ifdef IK_DBG_STATE_SEQUENCE
  void dbg_matrises::next_state(SCalculateData& cd) {
      if (old_dbg_m.size() > sdbg_state_sequence_number)
          old_dbg_m.erase(old_dbg_m.begin());
      old_dbg_m.push_back(dbg_m);
      Fmatrix m_dbg;
      dbg_m.OBJ = *cd.m_obj;
      dbg_m.ref_bone = cd.m_limb->ref_bone();
      if (cd.m_limb->ref_bone() == 2) {
  
          dbg_m.b2goal_gl = cd.state.goal;
          dbg_m.b3goal_gl.mul_43(dbg_m.b2goal_gl, cd.m_limb->transform(m_dbg, 2, 3));
  
          dbg_m.b2goal_lc.mul_43(Fmatrix().invert(*cd.m_obj), dbg_m.b2goal_gl);
          dbg_m.b3goal_lc.mul_43(Fmatrix().invert(*cd.m_obj), dbg_m.b3goal_gl);
          dbg_m.b3tob3_goal.mul(Fmatrix().invert(dbg_m.b2goal_gl), dbg_m.b3goal_gl);
  
          dbg_m.b2start_gl = cd.m_limb->sv_state.goal(m_dbg);
          dbg_m.b3start_gl.mulB_43(cd.m_limb->transform(m_dbg, 2, 3));
  
          dbg_m.b2start_lc.mul_43(Fmatrix().invert(*cd.m_obj), dbg_m.b2start_gl);
          dbg_m.b3start_lc.mul_43(Fmatrix().invert(*cd.m_obj), dbg_m.b3start_gl);
          dbg_m.b3tob3_start.mul(Fmatrix().invert(dbg_m.b2goal_gl), dbg_m.b3goal_gl);
  
      } else {
          dbg_m.b3goal_gl = cd.state.goal;
          dbg_m.b2goal_gl.mul_43(dbg_m.b3goal_gl, cd.m_limb->transform(m_dbg, 3, 2));
  
          dbg_m.b2goal_lc.mul_43(Fmatrix().invert(*cd.m_obj), dbg_m.b2goal_gl);
          dbg_m.b3goal_lc.mul_43(Fmatrix().invert(*cd.m_obj), dbg_m.b3goal_gl);
          dbg_m.b3tob3_goal.mul(Fmatrix().invert(dbg_m.b2goal_gl), dbg_m.b3goal_gl);
  
          dbg_m.b3start_gl = cd.m_limb->sv_state.goal(m_dbg);
          dbg_m.b2start_gl.mul_43(dbg_m.b3start_gl, cd.m_limb->transform(m_dbg, 3, 2));
  
          dbg_m.b2start_lc.mul_43(Fmatrix().invert(*cd.m_obj), dbg_m.b2start_gl);
          dbg_m.b3start_lc.mul_43(Fmatrix().invert(*cd.m_obj), dbg_m.b3start_gl);
          dbg_m.b3tob3_start.mul(Fmatrix().invert(dbg_m.b2goal_gl), dbg_m.b3goal_gl);
      }
  }
  
  void dbg_matrises::next_goal(const SCalculateData& cd) {
      Fmatrix goal;
      Matrix gl;
      dbg_m.dbg_goal = cd.goal(goal);
      dbg_m.GOAL = cvm(cd.m_limb->Goal(gl, goal, cd));
      dbg_m.OBJ_END = *cd.m_obj;
  }
  #endif