コード例 #1
0
ファイル: arcball.cpp プロジェクト: mmostajab/Vulkan
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;
}
コード例 #2
0
ファイル: mls.hpp プロジェクト: BITVoyager/pcl
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);
}
コード例 #3
0
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();
}
コード例 #4
0
// 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();
}
コード例 #5
0
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;
}
コード例 #6
0
ファイル: State.cpp プロジェクト: Shushman/dart
//==============================================================================
double State::getSagitalCOMDistance()
{
  Eigen::Vector3d xAxis = getCOMFrame().linear().col(0);  // x-axis
  Eigen::Vector3d d = getCOM() - getStanceAnklePosition();

  return d.dot(xAxis);
}
コード例 #7
0
ファイル: AnalyticEvaluate.hpp プロジェクト: miroi/pcmsolver
/*! \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;
}
コード例 #8
0
ファイル: State.cpp プロジェクト: Shushman/dart
//==============================================================================
double State::getCoronalCOMDistance()
{
  Eigen::Vector3d yAxis = getCOMFrame().linear().col(2);  // z-axis
  Eigen::Vector3d d = getCOM() - getStanceAnklePosition();

  return d.dot(yAxis);
}
コード例 #9
0
/** 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;
}
コード例 #10
0
ファイル: ObjectModelLine.cpp プロジェクト: brics/brics_3d
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);
	}
}
コード例 #11
0
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;
}
コード例 #12
0
        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;
            }
        }
コード例 #13
0
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;
}
コード例 #14
0
ファイル: vector3d_util.hpp プロジェクト: sfegan/calin
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;
}
コード例 #15
0
mesh_core::Plane::Plane(
      const Eigen::Vector3d& normal,
      const Eigen::Vector3d& point_on_plane)
  : normal_(normal.normalized())
{
  d_ = -point_on_plane.dot(normal_);
}
コード例 #16
0
  /*! This method calculates the angle between two vectors
     
  \warning If length() of any of the two vectors is == 0.0,
  this method will divide by zero. If the product of the
  length() of the two vectors is very close to 0.0, but not ==
  0.0, this method may behave in unexpected ways and return
  almost random results; details may depend on your particular
  floating point implementation. The use of this method is
  therefore highly discouraged, unless you are certain that the
  length()es are in a reasonable range, away from 0.0 (Stefan
  Kebekus)

  \deprecated This method will probably replaced by a safer
  algorithm in the future.

  \todo Replace this method with a more fool-proof version.

  @returns the angle in degrees (0-360)
  */
  OBAPI double VectorAngle (const Eigen::Vector3d& ab, const Eigen::Vector3d& bc)
  {
    // length of the two bonds
    const double l_ab = ab.norm();
    const double l_bc = bc.norm();
 
    if (IsNearZero(l_ab) || IsNearZero(l_bc)) {
      return 0.0;
    }

    // Calculate the cross product of v1 and v2, test if it has length unequal 0
    const Eigen::Vector3d c1 = ab.cross(bc);
    if (IsNearZero(c1.norm())) {
      return 0.0;
    }

    // Calculate the cos of theta and then theta
    const double dp = ab.dot(bc) / (l_ab * l_bc);
    if (dp > 1.0) {
      return 0.0;
    } else if (dp < -1.0) {
      return 180.0;
    } else {
      #ifdef USE_ACOS_LOOKUP_TABLE
      return (RAD_TO_DEG * acosLookup(dp));
      #else
      return (RAD_TO_DEG * acos(dp));
      #endif
    }
    
    return 0.0;
  }
コード例 #17
0
ファイル: ObjectModelLine.cpp プロジェクト: brics/brics_3d
bool ObjectModelLine::doSamplesVerifyModel (const std::set<int> &indices, const Eigen::VectorXd &model_coefficients,
		double threshold){

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

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

     double sqr_threshold = threshold * threshold;
     // Iterate through the 3d points and calculate the distances from them to the line
     for (std::set<int>::iterator it = indices.begin (); it != indices.end (); ++it)
     {
       // 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())[*it].getX(), (*inputPointCloud->getPointCloud())[*it].getY(),
    		   (*inputPointCloud->getPointCloud())[*it].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
       double sqr_distance = c.dot (c) / line_dir.dot (line_dir);

       if (sqr_distance > sqr_threshold)
         return (false);
     }

     return (true);
}
コード例 #18
0
ファイル: State.cpp プロジェクト: Shushman/dart
//==============================================================================
double State::getCoronalCOMVelocity()
{
  Eigen::Vector3d yAxis = getCOMFrame().linear().col(2);  // z-axis
  Eigen::Vector3d v = getCOMVelocity();

  return v.dot(yAxis);
}
コード例 #19
0
ファイル: State.cpp プロジェクト: Shushman/dart
//==============================================================================
double State::getSagitalCOMVelocity()
{
  Eigen::Vector3d xAxis = getCOMFrame().linear().col(0);  // x-axis
  Eigen::Vector3d v = getCOMVelocity();

  return v.dot(xAxis);
}
コード例 #20
0
double Face::distance(const Eigen::Vector3d& origin, const Eigen::Vector3d& direction) const
{
    // check for false intersection with the outside of the mesh
    if (!sameDirection(direction, normal().normalized())) {
        return INFINITY;
    }

    // Möller–Trumbore intersection algorithm
    const Eigen::Vector3d& p1(he->vertex->position);
    const Eigen::Vector3d& p2(he->next->vertex->position);
    const Eigen::Vector3d& p3(he->next->next->vertex->position);
    
    Eigen::Vector3d e1 = p2 - p1;
    Eigen::Vector3d e2 = p3 - p1;
    Eigen::Vector3d n = direction.cross(e2);
    
    double det = e1.dot(n);
    // ray does not lie in the plane
    if (det > -EPSILON && det < EPSILON) {
        return INFINITY;
    }
    
    double invDet = 1.0 / det;
    Eigen::Vector3d t = origin - p1;
    double u = t.dot(n) * invDet;
    
    // ray lies outside triangle
    if (u < 0.0 || u > 1.0) {
        return INFINITY;
    }
    
    Eigen::Vector3d q = t.cross(e1);
    double v = direction.dot(q) * invDet;
    // ray lies outside the triangle
    if (v < 0.0 || v + u > 1.0) {
        return INFINITY;
    }
    
    double s = e2.dot(q) * invDet;
    // intersection
    if (s > EPSILON) {
        return s;
    }
    
    // no hit
    return INFINITY;
}
コード例 #21
0
void mesh_core::Plane::leastSquaresGeneral(
      const EigenSTL::vector_Vector3d& points,
      Eigen::Vector3d* average)
{
  if (points.empty())
  {
    normal_ = Eigen::Vector3d(0,0,1);
    d_ = 0;
    if (average)
      *average = Eigen::Vector3d::Zero();
    return;
  }

  // find c, the average of the points
  Eigen::Vector3d c;
  c.setZero();

  EigenSTL::vector_Vector3d::const_iterator p = points.begin();
  EigenSTL::vector_Vector3d::const_iterator end = points.end();
  for ( ; p != end ; ++p)
    c += *p;

  c *= 1.0/double(points.size());

  // Find the matrix
  Eigen::Matrix3d m;
  m.setZero();

  p = points.begin();
  for ( ; p != end ; ++p)
  {
    Eigen::Vector3d cp = *p - c;
    m(0,0) += cp.x() * cp.x();
    m(1,0) += cp.x() * cp.y();
    m(2,0) += cp.x() * cp.z();
    m(1,1) += cp.y() * cp.y();
    m(2,1) += cp.y() * cp.z();
    m(2,2) += cp.z() * cp.z();
  }
  m(0,1) = m(1,0);
  m(0,2) = m(2,0);
  m(1,2) = m(2,1);

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(m);
  if (eigensolver.info() == Eigen::Success)
  {
    normal_ = eigensolver.eigenvectors().col(0);
    normal_.normalize();
  }
  else
  {
    normal_ = Eigen::Vector3d(0,0,1);
  }

  d_ = -c.dot(normal_);

  if (average)
    *average = c;
}
コード例 #22
0
ファイル: great_circle.cpp プロジェクト: WangFei-DUT/snark
great_circle::arc great_circle::arc::shortest_path( const coordinates &c ) const
{
    Eigen::Vector3d cc(c.to_cartesian());
    if (angle() == 0) { return arc(cc, begin()); }
    if (has_inside(c)) { return arc(cc, cc); }

    Eigen::Vector3d n = end() - begin();
    Eigen::Vector3d p((n*(n.dot(cc - begin())))/n.dot(n) + begin());
    boost::optional< coordinates > t = intersection_with(arc(p, cc));
    if (! t)
    {
        arc a1(cc, begin());
        arc a2(cc, end());
        return a1.angle() < a2.angle() ? a1 : a2;
    }
    return arc(cc, *t);
}
コード例 #23
0
double LinearAlgebra::dotProduct(const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
	double result;

	// TODO: return the dot product of vectors a and b.
	result = a.dot(b);

	return result;
}
コード例 #24
0
bool
PlaneObstacle::bounce(Eigen::Vector3d& x, Eigen::Vector3d& v) const
{
    double d = x.dot(mNormal);
    if (d > mOffset) return false;

    // Project the position onto the surface of the obstacle.
    x += (mOffset - d) * mNormal;

    // Split the velocity into normal and tangential components.
    double normMag = v.dot(mNormal);
    Eigen::Vector3d normVel = normMag * mNormal;
    Eigen::Vector3d tanVel = v - normVel;

    v = (1.0 - friction()) * tanVel;
    return true;
}
コード例 #25
0
ファイル: DefaultEventHandler.cpp プロジェクト: jpgr87/dart
//==============================================================================
Eigen::Vector3d DefaultEventHandler::getDeltaCursor(
    const Eigen::Vector3d& _fromPosition,
    ConstraintType _constraint,
    const Eigen::Vector3d& _constraintVector) const
{
    osg::Vec3d eye, center, up;
    mViewer->getCamera()->getViewMatrixAsLookAt(eye, center, up);

    Eigen::Vector3d near, far;
    getNearAndFarPointUnderCursor(near, far);
    Eigen::Vector3d v1 = far-near;

    if(LINE_CONSTRAINT == _constraint)
    {
        const Eigen::Vector3d& b1 = near;
        const Eigen::Vector3d& v2 = _constraintVector;
        const Eigen::Vector3d& b2 = _fromPosition;

        double v1_v1 = v1.dot(v1);
        double v2_v2 = v2.dot(v2);
        double v2_v1 = v2.dot(v1);

        double denominator = v1_v1*v2_v2 - v2_v1*v2_v1;
        double s;
        if(fabs(denominator) < 1e-10)
            s = 0;
        else
            s = (v1_v1*(v2.dot(b1)-v2.dot(b2)) + v2_v1*(v1.dot(b2)-v1.dot(b1)))/denominator;

        return v2*s;
    }
    else if(PLANE_CONSTRAINT == _constraint)
    {
        const Eigen::Vector3d& n = _constraintVector;
        double s = n.dot(_fromPosition - near) / n.dot(v1);
        return near - _fromPosition + s*v1;
    }
    else
    {
        Eigen::Vector3d n = osgToEigVec3(center - eye);
        double s = n.dot(_fromPosition - near) / n.dot(v1);
        return near - _fromPosition + s*v1;
    }

    return Eigen::Vector3d::Zero();
}
コード例 #26
0
double signedAngleBetween(const Eigen::Vector3d& dir1, const Eigen::Vector3d& dir2, const Eigen::Vector3d& norm)
{
	double dot = dir1.dot(dir2);

	if(dot < -1.0)
		dot = -1.0;
	if(dot > 1.0)
		dot = 1.0;

	double angle = acos(dot);

	Eigen::Vector3d cross = dir1.cross(dir2);
	double sign = norm.dot(cross);
	if(sign < 0)
		angle *= -1.0;

	return angle;
}
コード例 #27
0
Eigen::Vector3d get_earth_dipole(const Eigen::Vector3d& position)
{
	const double distance = position.norm();
	const Eigen::Vector3d
		direction = position / distance,
		projected_dip_mom = dipole_moment.dot(direction) * direction;

	return 1e-7 * (3 * projected_dip_mom - dipole_moment) / std::pow(distance, 3);
}
コード例 #28
0
ファイル: transform3d.cpp プロジェクト: TzarIvan/topo-blend
double errorOfCoplanar(Eigen::Vector3d &pt1, Eigen::Vector3d &normal1, Eigen::Vector3d &pt2, Eigen::Vector3d &normal2)
{
    double err1 = 1-std::abs(normal1.dot(normal2));

    Point_3 point(pt1.x(), pt1.y(), pt1.z());
    Plane_3 p2(Point_3(pt2.x(), pt2.y(), pt2.z()), Vector_3(normal2.x(), normal2.y(), normal2.z()));
    double dist1 = squared_distance(p2, point);
    return err1 + sqrt(dist1);
}
コード例 #29
0
 /* Calculate the distance of point a to the plane determined by b,c,d */
 double Point2Plane(const Eigen::Vector3d& a, const Eigen::Vector3d& b, 
     const Eigen::Vector3d& c, const Eigen::Vector3d& d)
 {
   Eigen::Vector3d ab = a - b;
   Eigen::Vector3d bc = c - b;
   Eigen::Vector3d bd = d - b;
   Eigen::Vector3d normal = bc.cross(bd);
   return fabs( normal.dot( ab ) / normal.norm() );
 }
コード例 #30
0
ファイル: planefit.cpp プロジェクト: AIS-Bonn/humanoid_op_ros
// Fit a plane to 3D data
void PlaneFit::fitPlane(const Points3D& P, Eigen::Vector4d& coeff, Eigen::Vector3d& mean)
{
	// Calculate a point on the plane and the normal vector to it
	Eigen::Vector3d normal;
	fitPlane(P, normal, mean);

	// Construct the coefficient vector (a,b,c,d) where the equation of the plane is ax + by + cz + d = 0
	coeff << normal, -normal.dot(mean);
}