C++ Vector3d::normalized方法代码示例

    /*
     * Creates a rotation matrix which describes how a point in an auxiliary
     * coordinate system, whose x axis is desbibed by vec_along_x_axis and has
     * a point on its xz-plane vec_on_xz_plane, rotates into the real coordinate
     * system.
     */
    void Cornea::createRotationMatrix(const Eigen::Vector3d &vec_along_x_axis,
                                      const Eigen::Vector3d &vec_on_xz_plane,
                                      Eigen::Matrix3d &R) {

        // normalise pw
        Eigen::Vector3d vec_on_xz_plane_n = vec_on_xz_plane.normalized();

        // define helper variables x, y and z
        // x
        Eigen::Vector3d xn = vec_along_x_axis.normalized();

        // y
        Eigen::Vector3d tmp = vec_on_xz_plane_n.cross(xn);
        Eigen::Vector3d yn = tmp.normalized();

        // z
        tmp = xn.cross(yn);
        Eigen::Vector3d zn = tmp.normalized();

        // create the rotation matrix
        R.col(0) << xn(0), xn(1), xn(2);
        R.col(1) << yn(0), yn(1), yn(2);
        R.col(2) << zn(0), zn(1), zn(2);

    }

    Eigen::Vector3d getForcePoint(Eigen::Vector3d & c_current, Eigen::Vector3d & c_previous, const double & ro)
    {
        if(c_current.norm()<ro)
        {
            Eigen::Vector3d f=kp_mat*(ro-c_current.norm())*c_current.normalized()
                    -kd_mat*(c_current.norm()-c_previous.norm())*c_current.normalized();

            return f;
        }
        else
        {
            return Eigen::Vector3d(0,0,0);
        }
    }

void PlanarJoint::setPlane(const Eigen::Vector3d& _rotAxis,
                            const Eigen::Vector3d& _tranAxis1)
{
  mPlaneType = PT_ARBITRARY;

  // Rotational axis
  mRotAxis = _rotAxis.normalized();

  // Translational axes
  assert(_rotAxis == _tranAxis1);
  Eigen::Vector3d unitTA1 = _tranAxis1.normalized();
  mTranAxis1 = (unitTA1 - unitTA1.dot(mRotAxis) * mRotAxis).normalized();

  mTranAxis2 = (mRotAxis.cross(mTranAxis1)).normalized();
}

//==============================================================================
ScrewJointUniqueProperties::ScrewJointUniqueProperties(
    const Eigen::Vector3d& _axis, double _pitch)
  : mAxis(_axis.normalized()),
    mPitch(_pitch)
{
  // Do nothing
}

//==============================================================================
PlaneShape::PlaneShape(const Eigen::Vector3d& _normal,
                       const Eigen::Vector3d& _point)
  : Shape(),
    mNormal(_normal.normalized()),
    mOffset(mNormal.dot(_point))
{
}

/** handleRayPick is called to test whether a visualizer is intersected
  * by a pick ray. It should be overridden by any pickable visualizer.
  *
  * \param pickOrigin origin of the pick ray in local coordinates
  * \param pickDirection the normalized pick ray direction, in local coordinates
  * \param pixelAngle angle in radians subtended by one pixel of the viewport
  */
bool
Visualizer::handleRayPick(const Eigen::Vector3d& pickOrigin,
                          const Eigen::Vector3d& pickDirection,
                          double pixelAngle) const
{
    if (!m_geometry.isNull())
    {
        // For geometry with a fixed apparent size, test to see if the pick ray is
        // within apparent size / 2 pixels of the center.
        if (m_geometry->hasFixedApparentSize())
        {
            double cosAngle = pickDirection.dot(-pickOrigin.normalized());
            if (cosAngle > 0.0)
            {

                if (cosAngle >= 1.0 || acos(cosAngle) < m_geometry->apparentSize() / 2.0 * pixelAngle)
                {
                    return true;
                }
            }
        }
    }

    return false;
}

//==============================================================================
Eigen::Isometry3d State::getCOMFrame() const
{
  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

  // Y-axis
  Eigen::Vector3d yAxis = Eigen::Vector3d::UnitY();

  // X-axis
  Eigen::Vector3d xAxis = mPelvis->getTransform().linear().col(0);
  Eigen::Vector3d pelvisXAxis = mPelvis->getTransform().linear().col(0);
  double mag = yAxis.dot(pelvisXAxis);
  pelvisXAxis -= mag * yAxis;
  xAxis = pelvisXAxis.normalized();

  // Z-axis
  Eigen::Vector3d zAxis = xAxis.cross(yAxis);

  T.translation() = getCOM();

  T.linear().col(0) = xAxis;
  T.linear().col(1) = yAxis;
  T.linear().col(2) = zAxis;

  return T;
}

mesh_core::Plane::Plane(
      const Eigen::Vector3d& normal,
      const Eigen::Vector3d& point_on_plane)
  : normal_(normal.normalized())
{
  d_ = -point_on_plane.dot(normal_);
}

// return closest point on line segment to the given point, and the distance
// betweeen them.
double mesh_core::closestPointOnLine(
      const Eigen::Vector3d& line_a,
      const Eigen::Vector3d& line_b,
      const Eigen::Vector3d& point,
      Eigen::Vector3d& closest_point)
{
  Eigen::Vector3d ab = line_b - line_a;
  Eigen::Vector3d ab_norm = ab.normalized();
  Eigen::Vector3d ap = point - line_a;

  Eigen::Vector3d closest_point_rel = ab_norm.dot(ap) * ab_norm;

  double dp = ab.dot(closest_point_rel);
  if (dp < 0.0)
  {
    closest_point = line_a;
  }
  else if (dp > ab.squaredNorm())
  {
    closest_point = line_b;
  }
  else
  {
    closest_point = line_a + closest_point_rel;
  }

  return (closest_point - point).norm();
}

void pointAndLineTransform(const Eigen::Vector3d& lineVector,
                           const Eigen::Vector3d& linePoint,
                           Eigen::Matrix4d& transform)
{
  //define a plane containing the line point and with normal vector equal to the line vector
  pcl::ModelCoefficientsPtr planeCoefficients( new pcl::ModelCoefficients );

  planeCoefficients->values.resize(4);

  Eigen::Vector3d normal;
  normal = lineVector.normalized();
  planeCoefficients->values[0] = normal(0,0);
  planeCoefficients->values[1] = normal(1,0);
  planeCoefficients->values[2] = normal(2,0);
  // Provided the plane explicit equation A·X + B·Y + C·Z + D = 0, define D by
  // evaluating the equation with the line point:
  planeCoefficients->values[3] = -( normal(0,0)*linePoint(0,0) +
                                    normal(1,0)*linePoint(1,0) +
                                    normal(2,0)*linePoint(2,0) );

  Eigen::Vector3d origin;
  origin = linePoint;

  pal::planeTransform(planeCoefficients,
                      &origin,
                      transform);
}

void Camera::updatePrincipleRay() {
	// XXX Does lens distortion play a role here...?
	const Eigen::Vector3d tcol = K_.col(2);
	const Eigen::Vector3d dir = Kinv_ * (tcol / tcol[2]);
	principleRay_.setSource(C_);
	principleRay_.setDirection(Rinv_ * dir.normalized());
}

 double constraints() {
     Eigen::Vector3d vec = ((*s2).curPos - (*s1).curPos);
     double magnitude = vec.norm();
     double ext_dist = magnitude - const_dist;
     // lighter objects move further
     double weight1 = fmax(1.0 - (*s1).mass/((*s1).mass + (*s2).mass), 1e-5);
     double weight2 = fmax(1.0 - (*s2).mass/((*s1).mass + (*s2).mass), 1e-5);
     if (abs(ext_dist) > 1e-5) {
         Eigen::Vector3d s1s2 = vec.normalized();
         Eigen::Vector3d s2s1 = -(vec.normalized());
         (*s1).curPos = (*s1).curPos + weight1*ext_dist*s1s2;
         (*s2).curPos = (*s2).curPos + weight2*ext_dist*s2s1;
         (*s1).oldPos = (*s1).oldPos + 0.6*weight1*ext_dist*s1s2;
         (*s2).oldPos = (*s2).oldPos + 0.6*weight2*ext_dist*s2s1;
     }
     return ext_dist;
 }

UniversalJoint::UniversalJoint(const Eigen::Vector3d& _axis0,
                               const Eigen::Vector3d& _axis1,
                               const std::string& _name)
  : Joint(_name)
{
  mGenCoords.push_back(&mCoordinate[0]);
  mGenCoords.push_back(&mCoordinate[1]);

  mS = Eigen::Matrix<double, 6, 2>::Zero();
  mdS = Eigen::Matrix<double, 6, 2>::Zero();

  mSpringStiffness.resize(2, 0.0);
  mDampingCoefficient.resize(2, 0.0);
  mRestPosition.resize(2, 0.0);

  mAxis[0] = _axis0.normalized();
  mAxis[1] = _axis1.normalized();
}

// Set this to a random transformation with bounded rotation and translation.
inline void Transformation::setRandom(double translationMaxMeters,
                                      double rotationMaxRadians) {
  // Create a random unit-length axis.
  Eigen::Vector3d axis = rotationMaxRadians * Eigen::Vector3d::Random();
  // Create a random rotation angle in radians.
  Eigen::Vector3d r = translationMaxMeters * Eigen::Vector3d::Random();
  r_ = r;
  q_ = Eigen::AngleAxisd(axis.norm(), axis.normalized());
  updateC();
}

void StateEstimatorKinematic::integrate_angular_velocity(Eigen::Vector3d xyz) {
    Eigen::Quaterniond m;
    // m = Eigen::AngleAxisd(timestep,xyz);

    m = Eigen::AngleAxisd(xyz.norm()*timestep,xyz.normalized());



    _q = _q*m;
}