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

void doArcball(
	double * _viewMatrix, 
	double const * _rotationCenter, 
	double const * _projectionMatrix, 
	double const * _initialViewMatrix, 
	//double const * _currentViewMatrix,
	double const * _initialMouse, 
	double const * _currentMouse, 
	bool screenSpaceRadius,
	double radius)
{
	Eigen::Map<Eigen::Vector3d const> rotationCenter(_rotationCenter);
	Eigen::Map<Eigen::Matrix4d const> initialViewMatrix(_initialViewMatrix);
	//Eigen::Map<Eigen::Matrix4d const> currentViewMatrix(_currentViewMatrix);
	Eigen::Map<Eigen::Matrix4d const> projectionMatrix(_projectionMatrix);
	Eigen::Map<Eigen::Matrix4d> viewMatrix(_viewMatrix);

	Eigen::Vector2d initialMouse(_initialMouse);
	Eigen::Vector2d currentMouse(_currentMouse);
	
	Eigen::Quaterniond q;
	Eigen::Vector3d Pa, Pc;
	if (screenSpaceRadius) {
		double aspectRatio = projectionMatrix(1, 1) / projectionMatrix(0, 0);

		initialMouse(0) *= aspectRatio;
		currentMouse(0) *= aspectRatio;

		Pa = mapToSphere(initialMouse, radius);
		Pc = mapToSphere(currentMouse, radius);

		q.setFromTwoVectors(Pa - rotationCenter, Pc - rotationCenter);
	}
	else {
		Pa = mapToSphere(projectionMatrix.inverse(), initialMouse, rotationCenter, radius);
		Pc = mapToSphere(projectionMatrix.inverse(), currentMouse, rotationCenter, radius);

		Eigen::Vector3d a = Pa - rotationCenter, b = Pc - rotationCenter;

#if 0
		std::cout << "Mouse Initial Radius = " << sqrt(a.dot(a)) << " Current Radius = " << sqrt(b.dot(b)) << std::endl;
#endif

		q.setFromTwoVectors(a, b);
	}	

	Eigen::Matrix4d qMat4;
	qMat4.setIdentity();
	qMat4.topLeftCorner<3, 3>() = q.toRotationMatrix();

	Eigen::Matrix4d translate;
	translate.setIdentity();
	translate.topRightCorner<3, 1>() = -rotationCenter;

	Eigen::Matrix4d invTranslate;
	invTranslate.setIdentity();
	invTranslate.topRightCorner<3, 1>() = rotationCenter;
	
    viewMatrix = invTranslate * qMat4 * translate * initialViewMatrix;
}

void
pcl::MLSResult::getMLSCoordinates (const Eigen::Vector3d &pt, double &u, double &v) const
{
  Eigen::Vector3d delta = pt - mean;
  u = delta.dot (u_axis);
  v = delta.dot (v_axis);
}

void LeePositionController::CalculateRotorVelocities(Eigen::VectorXd* rotor_velocities) const {
  assert(rotor_velocities);
  assert(initialized_params_);

  rotor_velocities->resize(vehicle_parameters_.rotor_configuration_.rotors.size());
  // Return 0 velocities on all rotors, until the first command is received.
  if (!controller_active_) {
    *rotor_velocities = Eigen::VectorXd::Zero(rotor_velocities->rows());
    return;
  }

  Eigen::Vector3d acceleration;
  ComputeDesiredAcceleration(&acceleration);

  Eigen::Vector3d angular_acceleration;
  ComputeDesiredAngularAcc(acceleration, &angular_acceleration);

  // Project thrust onto body z axis.
  double thrust = -vehicle_parameters_.mass_ * acceleration.dot(odometry_.orientation.toRotationMatrix().col(2));

  Eigen::Vector4d angular_acceleration_thrust;
  angular_acceleration_thrust.block<3, 1>(0, 0) = angular_acceleration;
  angular_acceleration_thrust(3) = thrust;

  *rotor_velocities = angular_acc_to_rotor_velocities_ * angular_acceleration_thrust;
  *rotor_velocities = rotor_velocities->cwiseMax(Eigen::VectorXd::Zero(rotor_velocities->rows()));
  *rotor_velocities = rotor_velocities->cwiseSqrt();
}

// 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 TetrahedronMesh::InitMesh()
{
  UpdateMesh();

  // Find min tet volume
  double minVol = std::numeric_limits<double>::max();
  for(int t=0;t<Tetrahedra->rows();t++)
  {
    Eigen::Vector3d A = InitalVertices->row(Tetrahedra->coeff(t,0)).cast<double>();
    Eigen::Vector3d B = InitalVertices->row(Tetrahedra->coeff(t,1)).cast<double>();
    Eigen::Vector3d C = InitalVertices->row(Tetrahedra->coeff(t,2)).cast<double>();
    Eigen::Vector3d D = InitalVertices->row(Tetrahedra->coeff(t,3)).cast<double>();

    Eigen::Vector3d a = A-D;
    Eigen::Vector3d b = B-D;
    Eigen::Vector3d c = C-D;

    double vol = a.dot(c.cross(b));

    if(vol < minVol)
      minVol = vol;
  }

  EPS1 = 10e-5;
  EPS3 = minVol*EPS1;
}

//==============================================================================
double State::getSagitalCOMDistance()
{
  Eigen::Vector3d xAxis = getCOMFrame().linear().col(0);  // x-axis
  Eigen::Vector3d d = getCOM() - getStanceAnklePosition();

  return d.dot(xAxis);
}

/*! \brief Analytic evaluation of ionic liquid Green's function and its derivatives
 *
 *  \f[
 *  \begin{align}
 *  \phantom{G(\vect{r},\vect{r}^\prime)}
 *  &\begin{aligned}
 *    G(\vect{r},\vect{r}^\prime) = \frac{\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|}
 *  \end{aligned}\\
 *  &\begin{aligned}
 *    \pderiv{}{{\vect{n}_{\vect{r}^\prime}}}G(\vect{r},\vect{r}^\prime) =
 *    \frac{(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}^\prime}\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^3}
 *    +\kappa\frac{(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}^\prime}\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^2}
 *   \end{aligned}\\
 *   &\begin{aligned}
 *     \pderiv{}{{\vect{n}_{\vect{r}}}}G(\vect{r},\vect{r}^\prime) =
 *     -\frac{(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}}\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^3}
 *     -\kappa\frac{(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}}\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^2}
 *    \end{aligned}\\
 *   &\begin{aligned}
 *     \frac{\partial^2}{\partial{\vect{n}_{\vect{r}}}\partial{\vect{n}_{\vect{r}^\prime}}}G(\vect{r},\vect{r}^\prime) &=
 *     \frac{\vect{n}_{\vect{r}}\cdot \vect{n}_{\vect{r}^\prime}\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^3}
 *     -\kappa\frac{[(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}}][(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}^\prime}]\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^4}\\
 *     &-3\frac{[(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}}][(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}^\prime}]\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^5}
 *     + \kappa\frac{\vect{n}_{\vect{r}}\cdot \vect{n}_{\vect{r}^\prime}\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^2} \\
 *     &-\kappa^2\frac{[(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}}][(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}^\prime}]\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^3}\\
 *     &-2\kappa\frac{[(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}}][(\vect{r}-\vect{r}^\prime)\cdot \vect{n}_{\vect{r}^\prime}]\mathrm{e}^{-\kappa|\vect{r}-\vect{r}^\prime|}}{4\pi\diel|\vect{r}-\vect{r}^\prime|^4}
 *    \end{aligned}
 *  \end{align}
 *  \f]
 */
inline Eigen::Array4d analyticIonicLiquid(double eps, double k,
        const Eigen::Vector3d & spNormal,
        const Eigen::Vector3d & sp,
        const Eigen::Vector3d & ppNormal, const Eigen::Vector3d & pp) {
    Eigen::Array4d result = Eigen::Array4d::Zero();
    double distance = (sp - pp).norm();
    double distance_3 = std::pow(distance, 3);
    double distance_5 = std::pow(distance, 5);

    // Value of the function
    result(0) = std::exp(- k * distance) / (eps * distance);
    // Value of the directional derivative wrt probe
    result(1) = (sp - pp).dot(ppNormal) * (1 + k * distance ) * std::exp(
                    - k * distance) / (eps * distance_3);
    // Directional derivative wrt source
    result(2) = - (sp - pp).dot(spNormal) * (1 + k * distance ) * std::exp(
                    - k * distance) / (eps * distance_3);
    // Value of the Hessian
    result(3) = spNormal.dot(ppNormal) * (1 + k * distance) * std::exp(
                    - k * distance) / (eps * distance_3)
                - std::pow(k, 2) * (sp - pp).dot(spNormal) * (sp - pp).dot(
                    ppNormal) * std::exp(- k * distance) / (eps * distance_3)
                - 3 * (sp - pp).dot(spNormal) * (sp - pp).dot(
                    ppNormal) * (1 + k * distance) * std::exp(- k * distance) /
                (eps * distance_5);

    return result;
}

//==============================================================================
double State::getCoronalCOMDistance()
{
  Eigen::Vector3d yAxis = getCOMFrame().linear().col(2);  // z-axis
  Eigen::Vector3d d = getCOM() - getStanceAnklePosition();

  return d.dot(yAxis);
}

/** 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;
}

void ObjectModelLine::getInlierDistance (std::vector<int> &inliers, const Eigen::VectorXd &model_coefficients,
		std::vector<double> &distances){

	assert (model_coefficients.size () == 6);

	distances.resize (this->inputPointCloud->getSize());

	// Obtain the line point and direction
	Eigen::Vector4d line_pt  (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
	Eigen::Vector4d line_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
	Eigen::Vector4d line_p2 = line_pt + line_dir;

	// Iterate through the 3d points and calculate the distances from them to the line
	for (size_t i = 0; i < this->inputPointCloud->getSize(); ++i)
	{
		// Calculate the distance from the point to the line
		// D = ||(P2-P1) x (P1-P0)|| / ||P2-P1|| = norm (cross (p2-p1, p2-p0)) / norm(p2-p1)
		Eigen::Vector4d pt ((*inputPointCloud->getPointCloud())[inliers[i]].getX(), (*inputPointCloud->getPointCloud())[inliers[i]].getY(),
				(*inputPointCloud->getPointCloud())[inliers[i]].getZ(), 0);
		Eigen::Vector4d pp = line_p2 - pt;

#ifdef EIGEN3
		Eigen::Vector3d c = pp.head<3> ().cross (line_dir.head<3> ());
#else
		Eigen::Vector3d c = pp.start<3> ().cross (line_dir.start<3> ());
#endif
		//distances[i] = sqrt (c.dot (c)) / line_dir.dot (line_dir);
		distances[i] = c.dot (c) / line_dir.dot (line_dir);
	}
}

void SubMoleculeTest::rotate()
{
  SubMolecule *sub = m_source_h2o->getRandomSubMolecule();

  // Rotate into xy-plane: Align the cross product of the bond vectors
  // with the z-axis
  Q_ASSERT(sub->numBonds() == 2);
  const Eigen::Vector3d b1= *sub->bond(0)->beginPos()-*sub->bond(0)->endPos();
  const Eigen::Vector3d b2= *sub->bond(1)->beginPos()-*sub->bond(1)->endPos();
  const Eigen::Vector3d cross = b1.cross(b2).normalized();

  // Axis is the cross-product of cross with zhat:
  const Eigen::Vector3d axis = cross.cross(Eigen::Vector3d::UnitZ()).normalized();

  // Angle is the angle between cross and jhat:
  const double angle = acos(cross.dot(Eigen::Vector3d::UnitZ()));

  // Rotate the submolecule
  sub->rotate(angle, axis);

  // Verify that the molecule is in the xy-plane
  QVERIFY(fabs(sub->atom(0)->pos()->z()) < 1e-2);
  QVERIFY(fabs(sub->atom(1)->pos()->z()) < 1e-2);
  QVERIFY(fabs(sub->atom(2)->pos()->z()) < 1e-2);
  delete sub;
}

        void constraints() {
            //Variables for max angle position
            Eigen::Vector3d maxPos;
            Eigen::Matrix3d rotation;
            Eigen::Vector3d diff;
            double cosa, sina, mcosa, msina;


            Eigen::Vector3d hingeCenter = (s2->curPos + s3->curPos)/2;
            Eigen::Vector3d s2s3 = (s2->curPos - s3->curPos).normalized();
            Eigen::Vector3d v1 = s4->curPos - hingeCenter;
            Eigen::Vector3d v2 = s1->curPos - hingeCenter;
            //Need the up vector to enlarge to 360 degrees
            Eigen::Vector3d up = s2s3.cross(v2);
            //Calculated using the dotproduct
            double orientation = up.dot(v2);
            //use dotproduct cosine relation to find angle (in degrees), unfortunately limited to 0-180 degrees so need
            //to use up vector (calculated above) to enlarge to 360 degrees.
            double angle = acos(v1.dot(v2)/(v1.norm() * v2.norm())) * 180/3.1415926535;
            if ( orientation < 0 ) {
                angle = angle + 180;
            }
            if (!(abs(angle - const_angle) < 1e-3)) {
                double rotation_amount = const_angle - angle;
                cosa = cos(rotation_amount * 3.1415926535/180); sina = sin(rotation_amount * 3.1415926535/180); mcosa = 1 - cosa; msina = 1 - sina;
                rotation << 
                    cosa + s2s3[0] * s2s3[0] * mcosa, s2s3[0] * s2s3[1] * mcosa - s2s3[2] * sina, s2s3[0] * s2s3[2] * mcosa + s2s3[1] * sina,
                    s2s3[1] * s2s3[0] * mcosa + s2s3[2] * sina, cosa + s2s3[1] * s2s3[1] * mcosa, s2s3[1] * s2s3[2] * mcosa - s2s3[0] * sina,
                    s2s3[2] * s2s3[0] * mcosa - s2s3[1] * sina, s2s3[2] * s2s3[1] * mcosa + s2s3[0] * sina, cosa + s2s3[2] * s2s3[2] * mcosa;
                maxPos = rotation * s1->curPos;
                diff = maxPos - s4->curPos;
                s4->curPos = maxPos;
                s4->oldPos = s4->oldPos + diff;
            }
        }

double connectionAngle(double angle, HalfEdgeIter& he)
{
    Eigen::Vector3d x, y;
    Eigen::Vector3d v = he->flip->vertex->position - he->vertex->position;
    if (he != he->edge->he) v = -v;
    
    // delta ij
    he->face->axis(x, y);
    double deltaIJ = atan2(v.dot(y), v.dot(x));
    
    // delta ji
    he->flip->face->axis(x, y);
    double deltaJI = atan2(v.dot(y), v.dot(x));
    
    return angle - deltaIJ + deltaJI;
}

inline Eigen::Vector3d reflect_from_surface(const Eigen::Vector3d& v,
    const Eigen::Vector3d& surface_norm)
{
  assert(fabs(surface_norm.norm() - 1.0) < 1e-6);
  const double mag = 2.0*v.dot(surface_norm);
  return v - surface_norm * mag;
}

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