C++ Matrix4x4f::Inverse方法代码示例

void BoneBridgeCAL3D::SetModelSpaceTransform(const Matrix4x4f& modelSpaceTransform) const
{
   // calculate the transform as relative to the parent bone
   Matrix4x4f localModelSpaceTransform = Matrix4x4f::Identity();
   {
      // for now use absolute model space positioning
      localModelSpaceTransform = modelSpaceTransform;

      // divide by any existing parent's model space position
      if (const Bone* parent = GetParent())
      {
         //const osg::Matrix parentTransform = ConvertKerneltoOSG(parent->GetModelSpaceTransform());
         const Matrix4x4f parentTransform = parent->GetModelSpaceTransform();

         // get inverse of parent transform
         const Matrix4x4f invParentTransform = parentTransform.Inverse();

         localModelSpaceTransform *= invParentTransform;
      }
   }

   // now update the cal3d side from this new model space information
#if WRITE_CAL3D_BONE_TRANSLATION
   // update model space position
   {
      Vec3f kernelPosVec = localModelSpaceTransform.GetTranslation();
      CalVector calPosVec;

      for (int i = 0; i < 3; ++i)
      {
         calPosVec[i] = kernelPosVec[i];
      }

      mpCalBone->setTranslation(calPosVec); // setting the relative position
   }
#endif

#if WRITE_CAL3D_BONE_ROTATION
   // update model space orientation
   {
      const Quaternionf& kernelRotQuat = localModelSpaceTransform.GetRotate();

      CalQuaternion calRotQuat;
      calRotQuat = ConvertKerneltoCAL3D(kernelRotQuat);

      mpCalBone->setRotation(calRotQuat); // setting the relative orientation
   }
#endif
}

std::vector<Vec3f> BoneBridgeCAL3D::ComputeBoundingBoxCorners(const CalBoundingBox& box) const
{
   std::vector<Vec3f> corners;

   // what we have is a set of 6 planes, stored as plane equation variables
   // what we need is 8 points for drawing a box
   // each point is the intersection of three of the planes
   // what we need to do is choose each set of 3 planes that intersects at a viable corner

   // begin at the beginning
   corners.clear();

   // iterate through all combinations of distinct planes a, b, c
   for (int a = 0; a < 6-2; ++a)
   {
      for (int b = a+1; b < 6-1; ++b)
      {
         // if a and b are parallel, skip
         if (PlanesParallel(box.plane[a], box.plane[b]))
         {
            continue;
         }

         for (int c = b+1; c < 6-0; ++c)
         {
            // the three planes indexed a, b, c potentially intersect at a viable corner

            // if any two are parallel, we should skip this trio
            // getting this far, we know a and b are not parallel -- let's check a,c and b,c
            if (PlanesParallel(box.plane[a], box.plane[c]) || PlanesParallel(box.plane[b], box.plane[c]))
            {
               continue;
            }

            // we have three orthogonal planes, they should intersect at a single point, serving as a corner to the box
            const Matrix4x4f M(
               box.plane[a].a, box.plane[a].b, box.plane[a].c, 0,
               box.plane[b].a, box.plane[b].b, box.plane[b].c, 0,
               box.plane[c].a, box.plane[c].b, box.plane[c].c, 0,
               0,              0,              0,              1);

            const Matrix4x4f Mi = M.Inverse();
            {
               const Vec4f offsets(
                  box.plane[a].d,
                  box.plane[b].d,
                  box.plane[c].d,
                  0.0f);

               const Vec4f corner = Mi * offsets;

               const Vec3f fixedCorner(corner[X], corner[Y], -corner[Z]);
               corners.push_back(fixedCorner);
            }
         }
      }
   }

   // make sure we're all good
   assert(corners.size() == 8);

   return corners;
}