C++ Quaternionf::inverse方法代码示例

Eigen::Quaternionf rollcorrect(Eigen::Quaternionf rotation){
    Vector3f local_z_axis = rotation.inverse() * -Vector3f::UnitZ();
    Vector3f local_x_axis = rotation.inverse() * Vector3f::UnitX();
    Vector3f local_y_axis = rotation.inverse() * Vector3f::UnitY();

    Vector3f z_proj_zx = local_z_axis; z_proj_zx(1) = 0; z_proj_zx.normalize();
    Vector3f x_proj_zx = local_x_axis; x_proj_zx(1) = 0; x_proj_zx.normalize();
    Vector3f y_proj_zx = local_y_axis; y_proj_zx(1) = 0; y_proj_zx.normalize();

    double pitch = angle(local_z_axis, z_proj_zx);
    double roll = angle(local_x_axis, x_proj_zx);

    int proj_side = local_x_axis.cross(x_proj_zx).dot(local_z_axis) > 0 ? -1 : 1;
    int side_up = local_y_axis.dot(Vector3f::UnitY()) > 0 ? 1 : -1;

    if(side_up == -1){
        roll = M_PI - roll;
    }

    roll = roll * proj_side * side_up;

    double norm_pitch = normalizeRad(pitch);
    double correction_factor = (M_PI_2 - fabs(norm_pitch)) / M_PI_2;
    correction_factor = pow(correction_factor - 0.5, 1);

    if(pitch > 0.7){
        correction_factor = 0;
    }

    AngleAxis<float> roll_correction(correction_factor*-roll, Vector3f::UnitZ());
    rotation = roll_correction * rotation;

    return rotation;
}

void BulletWrapper::updateObjectHolding(const SkeletonHand& skeletonHand, BulletHandRepresentation& handRepresentation, float deltaTime)
{
  const float strengthThreshold = 1.8f;
  if (skeletonHand.grabStrength >= strengthThreshold && handRepresentation.m_HeldBody == NULL)
  {
    // Find body to pick
    EigenTypes::Vector3f underHandDirection = -skeletonHand.rotationButNotReally * EigenTypes::Vector3f::UnitY();

    btRigidBody* closestBody = utilFindClosestBody(skeletonHand.getManipulationPoint(), underHandDirection);
    handRepresentation.m_HeldBody = closestBody;
  }

  if (skeletonHand.grabStrength < strengthThreshold)
  {
    // Let body go
    handRepresentation.m_HeldBody = NULL;
  }
  

  if (handRepresentation.m_HeldBody != NULL)
  {
    btRigidBody* body = handRepresentation.m_HeldBody;
    // Control held body
    s_LastHeldBody = body;
    m_FramesTilLastHeldBodyReset = 12;

    //handRepresentation.m_HeldBody->setCollisionFlags(0);

    // apply velocities or apply positions
    btVector3 target = ToBullet(skeletonHand.getManipulationPoint());// - skeletonHand.rotationButNotReally * EigenTypes::Vector3f(0.1f, 0.1f, 0.1f));
    btVector3 current = body->getWorldTransform().getOrigin();
    btVector3 targetVelocity = 0.5f * (target - current) / deltaTime;
    body->setLinearVelocity(targetVelocity);

    // convert from-to quaternions to angular velocity in world space
    {
      Eigen::Quaternionf currentRotation = FromBullet(body->getWorldTransform().getRotation());
      Eigen::Quaternionf targetRotation = Eigen::Quaternionf(skeletonHand.arbitraryRelatedRotation()); // breaks for left hand ???

      Eigen::Quaternionf delta = currentRotation.inverse() * targetRotation;
      Eigen::AngleAxis<float> angleAxis(delta);
      EigenTypes::Vector3f axis = angleAxis.axis();
      float angle = angleAxis.angle();
      const float pi = 3.1415926536f;
      if (angle > pi) angle -= 2.0f * pi;
      if (angle < -pi) angle += 2.0f * pi;

      EigenTypes::Vector3f angularVelocity = currentRotation * (axis * angle * 0.5f / deltaTime);
      body->setAngularVelocity(ToBullet(angularVelocity));
    }
  }
}

bool Geometry::GetMVBB(const Geometry::VertexSet &vertices, Box &box) {
    ApproxMVBB::Vector3List v;
    for (Geometry::VertexSet::const_iterator it = vertices.begin(); it!=vertices.end();++it){
        pcl::PointXYZ p;
        Vertex2Point(*it,p);
        v.emplace_back(p.x,p.y,p.z);
    }
    ApproxMVBB::Matrix3Dyn points(3,v.size());
    for(int i=0;i<v.size();i++)
        points.col(i)=v[i];
    //NOTE parameters see:https://github.com/gabyx/ApproxMVBB
    ApproxMVBB::OOBB oobb = ApproxMVBB::approximateMVBB(points,0.001,8,5,0,5);
    Eigen::Vector3f centroid(
            (const float &) ((oobb.m_minPoint.x()+oobb.m_maxPoint.x())/2),
            (const float &) ((oobb.m_minPoint.y()+oobb.m_maxPoint.y())/2),
            (const float &) ((oobb.m_minPoint.z()+oobb.m_maxPoint.z())/2));
    //upper/lower point in OOBB frame
    double width = oobb.m_maxPoint.x()-oobb.m_minPoint.x();
    double height = oobb.m_maxPoint.y()-oobb.m_minPoint.y();
    double depth = oobb.m_maxPoint.z()-oobb.m_minPoint.z();
    if (width<=0 || height<=0 ||depth<=0)
        return false;
//    coordinate transformation A_IK matrix from OOBB frame K to world frame I
    Eigen::Quaternionf q;
    q.x()= (float) oobb.m_q_KI.x();
    q.y()= (float) oobb.m_q_KI.y();
    q.z()= (float) oobb.m_q_KI.z();
    q.w()= (float) oobb.m_q_KI.w();
    centroid=q.matrix()*centroid;
//    a translation to apply to the cube from 0,0,0
    box.centroid=centroid;
//    a quaternion-based rotation to apply to the cube
    box.quanternion=q.inverse();
    box.depth=depth;
    box.height=height;
    box.width=width;
    return true;
}