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

INS INSInitializer::initializeINS() const {
    // Average up all accel and mag samples
    Eigen::Vector3d y_a = mean(Eigen::Vector3d::Zero(), y_a_log.begin(), y_a_log.end());
    Eigen::Vector3d y_m = mean(Eigen::Vector3d::Zero(), y_m_log.begin(), y_m_log.end());

    // Use TRIAD to compute a rotation matrix
    Eigen::Vector3d a = -config.g_nav.normalized();
    Eigen::Vector3d a_hat = y_a.normalized();
    Eigen::Vector3d m = a.cross(config.m_nav).normalized();
    Eigen::Vector3d m_hat = a_hat.cross(y_m).normalized();
    Eigen::Matrix3d R_imu2nav =
        (Eigen::Matrix3d() << a, m, a.cross(m)).finished() *
        (Eigen::Matrix3d() << a_hat, m_hat, a_hat.cross(m_hat)).finished().transpose();

    // Sum up all the DVL readings
    Eigen::Vector4d y_d = mean(Eigen::Vector4d::Zero(), y_d_log.begin(), y_d_log.end());

    // Use least squares with the beam matrix to determine our velocity
    Eigen::Vector3d v_imu_init =
        (config.beam_mat.transpose()*config.beam_mat).lu().solve( // TODO fix guide
            config.beam_mat.transpose() * y_d);

    // Use the depth sensor to determine our z position
    double p_nav_z = -mean(0, y_z_log.begin(), y_z_log.end())
        - (R_imu2nav*config.r_imu2depth)[2];

    // Initialize an INS
    return INS(Eigen::Quaterniond(R_imu2nav),
               R_imu2nav * v_imu_init,
               Eigen::Vector3d(0, 0, p_nav_z),
               config.g_nav);
}

  /*!  This function calculates the torsion angle of three vectors, represented
    by four points A--B--C--D, i.e. B and C are vertexes, but none of A--B,
    B--C, and C--D are colinear.  A "torsion angle" is the amount of "twist"
    or torsion needed around the B--C axis to bring A--B into the same plane
    as B--C--D.  The torsion is measured by "looking down" the vector B--C so
    that B is superimposed on C, then noting how far you'd have to rotate
    A--B to superimpose A over D.  Angles are + in theanticlockwise
    direction.  The operation is symmetrical in that if you reverse the image
    (look from C to B and rotate D over A), you get the same answer.
  */
  OBAPI double VectorTorsion(const Eigen::Vector3d& a, const Eigen::Vector3d& b,
      const Eigen::Vector3d& c, const Eigen::Vector3d& d)
  {
    // Bond vectors of the three atoms
    Eigen::Vector3d ab = b - a;
    Eigen::Vector3d bc = c - b;
    Eigen::Vector3d cd = d - c;

    // length of the three bonds
    const double l_ab = ab.norm();
    const double l_bc = bc.norm();
    const double l_cd = cd.norm();
    
    if (IsNearZero(l_ab) || IsNearZero(l_bc) || IsNearZero(l_cd) ) {
      return 0.0;
    }
 
    // normalize the bond vectors:
    ab *= (1.0 / l_ab);
    bc *= (1.0 / l_bc);
    cd *= (1.0 / l_cd);

    const Eigen::Vector3d ca = ab.cross(bc);
    const Eigen::Vector3d cb = bc.cross(cd);
    const Eigen::Vector3d cc = ca.cross(cb);
    const double d1 = cc.dot(bc);
    const double d2 = ca.dot(cb);
    const double tor = RAD_TO_DEG * atan2(d1, d2);
    
    return tor;  
  }

std::pair<Eigen::Vector3d, double> resolveCenterOfPressure(Eigen::Vector3d torque, Eigen::Vector3d force, Eigen::Vector3d normal, Eigen::Vector3d point_on_contact_plane)
{
  // TODO: implement multi-column version
  using namespace Eigen;

  if (abs(normal.squaredNorm() - 1.0) > 1e-12) {
    mexErrMsgIdAndTxt("Drake:resolveCenterOfPressure:BadInputs", "normal should be a unit vector");
  }

  Vector3d cop;
  double normal_torque_at_cop;

  double fz = normal.dot(force);
  bool cop_exists = abs(fz) > 1e-12;

  if (cop_exists) {
    auto torque_at_point_on_contact_plane = torque - point_on_contact_plane.cross(force);
    double normal_torque_at_point_on_contact_plane = normal.dot(torque_at_point_on_contact_plane);
    auto tangential_torque = torque_at_point_on_contact_plane - normal * normal_torque_at_point_on_contact_plane;
    cop = normal.cross(tangential_torque) / fz + point_on_contact_plane;
    auto torque_at_cop = torque - cop.cross(force);
    normal_torque_at_cop = normal.dot(torque_at_cop);
  }
  else {
    cop.setConstant(std::numeric_limits<double>::quiet_NaN());
    normal_torque_at_cop = std::numeric_limits<double>::quiet_NaN();
  }
  return std::pair<Vector3d, double>(cop, normal_torque_at_cop);
}

Eigen::Vector3d ManipTool::update_translation_est(Eigen::Vector3d lv,Eigen::Vector3d rv,\
                                       Eigen::Matrix3d robot_eef_rm, MyrmexTac *myrtac){
    Eigen::Matrix3d omiga_skmatrix;
    Eigen::Vector3d temp_lv;
    Eigen::Vector3d r_hat;
    r_hat.setZero();
    temp_lv.setZero();
    omiga_skmatrix.setZero();
    omiga_skmatrix = vectortoskew(rv);
    temp_lv(1) = myrtac->ctc_vel(0);
    temp_lv(0) = myrtac->ctc_vel(1);

    //get the vector from center of tactile sensor to the contact point
    r_hat(0) = myrtac->cog_y -7.5;
    r_hat(1) = myrtac->cog_x -7.5;

    L_r_dot = (-1)*beta_r*L_r-omiga_skmatrix*omiga_skmatrix;

    //compute the ve, comparing with Yannis's paper, I am using -v as the Tao.
    //because tao = r /cross (f), and v = omega /cross (r)

    c_r_dot = (-1)*beta_r*c_r+omiga_skmatrix*(-1)*((ts.tac_sensor_cfm_local*((-1)*temp_lv*0.005/0.004)-\
                                               rv.cross(ts.tac_sensor_cfm_local*r_hat*0.005)) - lv);
    est_trans_dot = (-1)*Gama_r*(L_r*est_trans-c_r);
    L_r = L_r + L_r_dot;
    c_r = c_r + c_r_dot;
    est_trans = est_trans + est_trans_dot;
    return ((ts.tac_sensor_cfm_local*(temp_lv*0.005/0.004)-\
             rv.cross(ts.tac_sensor_cfm_local*r_hat*0.005)) - lv);
}

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

 bool ConvexPolygon::isInside(const Eigen::Vector3d& p)
 {
   Eigen::Vector3d A0 = vertices_[0];
   Eigen::Vector3d B0 = vertices_[0 + 1];
   Eigen::Vector3d direction0 = (B0 - A0).normalized();
   Eigen::Vector3d direction20 = (p - A0).normalized();
   bool direction_way = direction0.cross(direction20).dot(normal_) > 0;
   for (size_t i = 1; i < vertices_.size() - 1; i++) {
     Eigen::Vector3d A = vertices_[i];
     Eigen::Vector3d B = vertices_[i + 1];
     Eigen::Vector3d direction = (B - A).normalized();
     Eigen::Vector3d direction2 = (p - A).normalized();
     if (direction_way) {
       if (direction.cross(direction2).dot(normal_) >= 0) {
         continue;
       }
       else {
         return false;
       }
     }
     else {
       if (direction.cross(direction2).dot(normal_) <= 0) {
         continue;
       }
       else {
         return false;
       }
     }
   }
   return true;
 }

void Optimizer::eachCloudPair(CloudPair &pair)
{
    int cloud0 = pair.corresIdx.first;
    int cloud1 = pair.corresIdx.second;

    size_t matrix_size = m_pointClouds.size() * 6;

    TriContainer mat_elem;
    mat_elem.reserve(matrix_size * matrix_size / 5);
    SparseMatFiller filler(mat_elem);

    for (int i = 0; i < 6; ++i) {
        filler.add(i, i, 1.0);
    }

    Vec atb(matrix_size);
    Mat ata(matrix_size, matrix_size);
    atb.setZero(), ata.setZero();

    double score = 0.0;
    {
        //pcl::ScopeTime time("calculate LSE matrix");
    
#pragma unroll 8
        for (size_t point_count = 0; point_count < pair.corresPointIdx.size(); ++point_count) {
            int point_p = pair.corresPointIdx[point_count].first;
            int point_q = pair.corresPointIdx[point_count].second;
            PointType P = m_pointClouds[cloud0]->points[point_p];
            PointType Q = m_pointClouds[cloud1]->points[point_q];

            Eigen::Vector3d p = P.getVector3fMap().cast<double>();
            Eigen::Vector3d q = Q.getVector3fMap().cast<double>();
            Eigen::Vector3d Np = P.getNormalVector3fMap().cast<double>();

            double b = -(p - q).dot(Np);
            score += b * b;
            Eigen::Matrix<double, 6, 1> A_p, A_q;
            A_p.block<3, 1>(0, 0) = p.cross(Np);
            A_p.block<3, 1>(3, 0) = Np;
            A_q.block<3, 1>(0, 0) = -q.cross(Np);
            A_q.block<3, 1>(3, 0) = -Np;
        
            filler.fill(cloud0, cloud1, A_p, A_q);
            atb.block<6, 1>(cloud0 * 6, 0) += A_p * b;
            atb.block<6, 1>(cloud1 * 6, 0) += A_q * b;
        }
        ata.setFromTriplets(mat_elem.begin(), mat_elem.end());
    }

    {
        //pcl::ScopeTime time("Fill sparse matrix");
        boost::mutex::scoped_lock lock(m_cloudPairMutex);
        //std::cout << "\tcurrent thread : " << boost::this_thread::get_id() << std::endl;
        //PCL_INFO("\tPair <%d, %d> alignment Score : %.6f\n", cloud0, cloud1, score);
        ATA += ata;
        ATb += atb;
        align_error += score;
    }
}

Eigen::Vector3d MetricRectification::normalizeLine(Eigen::Vector3d p0, Eigen::Vector3d p1)
{
	Eigen::Vector3d l = p0.cross(p1);
	l.x() = l.x() / l.z();
	l.y() = l.y() / l.z();
	l.z() = 1.0;
	//return l;
	return p0.cross(p1).normalized();
}

  OBAPI double VectorOOP(const Eigen::Vector3d &a, const Eigen::Vector3d &b,
      const Eigen::Vector3d &c,const Eigen::Vector3d &d)
  {
    // This is adapted from http://scidok.sulb.uni-saarland.de/volltexte/2007/1325/pdf/Dissertation_1544_Moll_Andr_2007.pdf
    // Many thanks to Andreas Moll and the BALLView developers for this

    // calculate normalized bond vectors from central atom to outer atoms:
    Eigen::Vector3d ab = a - b;
    // store length of this bond:
    const double length_ab = ab.norm();
    if (IsNearZero(length_ab)) {
      return 0.0;
    }
    // store the normalized bond vector from central atom to outer atoms:
    // normalize the bond vector:
    ab /= length_ab;
		
    Eigen::Vector3d cb = c - b;
    const double length_cb = cb.norm();
    if (IsNearZero(length_cb)) {
      return 0.0;
    }
    cb /= length_cb;
	
    Eigen::Vector3d db = d - b;
    const double length_db = db.norm();
    if (IsNearZero(length_db)) {
      return 0.0;
    }
    db /= length_db;
	
    // the normal vectors of the three planes:
    const Eigen::Vector3d an = ab.cross(cb); 
    const Eigen::Vector3d bn = cb.cross(db); 
    const Eigen::Vector3d cn = db.cross(ab); 

    // Bond angle ji to jk
    const double cos_theta = ab.dot(cb);
    #ifdef USE_ACOS_LOOKUP_TABLE
    const double theta = acosLookup(cos_theta);
    #else
    const double theta = acos(cos_theta);
    #endif
    // If theta equals 180 degree or 0 degree
    if (IsNearZero(theta) || IsNearZero(fabs(theta - M_PI))) {
      return 0.0;
    }
				
    const double sin_theta = sin(theta);
    const double sin_dl = an.dot(db) / sin_theta;

    // the wilson angle:
    const double dl = asin(sin_dl);

    return RAD_TO_DEG * dl;
  }

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

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

        // - line has starting point (x0, y0, z0) and ending point (x1, y1, z1) 
        bool LineIntersect(Eigen::Vector3d& line_start, Eigen::Vector3d& line_end,
                           Eigen::Vector3d& intersection) {
            // Solution : http://www.gamedev.net/community/forums/topic.asp?topic_id=467789
            Eigen::Vector3d line_dir = (line_end - line_start).normalized();
            Eigen::Vector3d A = this->node1->curPos;
            Eigen::Vector3d B = this->node2->curPos;

            Eigen::Vector3d nan_bias1(1e-10, -1e-10, 1e-10);
            Eigen::Vector3d nan_bias2(-1e-10, 1e-10, -1e-10);
            Eigen::Vector3d AB = (B - A)+nan_bias1;
            Eigen::Vector3d AO = (line_start - A)+nan_bias2;
            Eigen::Vector3d AOxAB = AO.cross(AB);
            Eigen::Vector3d VxAB  = line_dir.cross(AB);

            double ab2 = AB.dot(AB);
            double a = VxAB.dot(VxAB);
            double b = 2 * VxAB.dot(AOxAB);
            double c = AOxAB.dot(AOxAB) - (r*r * ab2);
            double d = b*b - 4*a*c;
            if (d < 0) 
                return false;
            double t = (-b - sqrt(d)) / (2 * a);
            if (t < 0) 
                return false;

            intersection = line_start + line_dir*t; /// intersection point
            Eigen::Vector3d projection = A + (AB.dot(intersection - A) / ab2) * AB; /// intersection projected onto cylinder axis
            if ((projection - A).norm() + (B - projection).norm() > AB.norm() + 1e-5) 
                return false;

            return true;
        }

      Eigen::Vector3d interpolate_3x3(const Eigen::Vector3d &this_vec, const double &time_now, const Eigen::Vector3d &next_vec, const double &time_other, const double &time_between)
      {
	Eigen::Quaternion<double> quat(1.0, 0.0, 0.0, 0.0);
	double this_norm = this_vec.norm();
	double next_norm = next_vec.norm();
	Eigen::Vector3d this_uv = unit_vec(this_vec);
	Eigen::Vector3d next_uv = unit_vec(next_vec);
	double x = this_uv.dot(next_uv);
	double y = limit(x,-1.0, 1.0);
	double theta = acos(y);
	double adjust = (time_between - time_now) / (time_other - time_now);
	double factor = ((next_norm - this_norm) * adjust + this_norm) / this_norm;
	theta = theta * adjust;
	double sto2 = sin(theta / 2.0);
	Eigen::Vector3d ax = next_vec.cross(this_vec);
	Eigen::Vector3d ax_uv = unit_vec(ax);
	double qx, qy, qz, qw;
	qx = ax_uv(0) * sto2;
	qy = ax_uv(1) * sto2;
	qz = ax_uv(2) * sto2;
	qw = cos(theta/2.0);
	quat = Eigen::Quaternion<double>(qw,qx,qy,qz);
	Eigen::Vector3d z = this_vec * factor;
	Eigen::Quaternion<double> z_q(0.0, z(0), z(1), z(2));
	Eigen::Quaternion<double> q1 = z_q * quat;
	Eigen::Quaternion<double> retaq = quat.inverse();
	Eigen::Quaternion<double> q2 = retaq * q1;
	return q2.vec();
      }

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

    }

//line triangle intersections
Eigen::Vector3d Triangle::shortestDistanceTo(Eigen::Vector3d line_segment_start, Eigen::Vector3d line_segment_end, Eigen::Vector3d& mesh_closest_point)
{
  Eigen::Vector3d shortest_dist(BIG_DOUBLE,BIG_DOUBLE,BIG_DOUBLE);

  //line intersection with all of the edges
  Eigen::Vector3d normal = ((points[1]-points[0]).cross(points[2]-points[1])).normalized();
  float d = points[1].dot(normal);
  Eigen::Vector3d p = line_segment_start - (line_segment_start.dot(normal) -d)*normal;
  Eigen::Vector3d r = (line_segment_end - (line_segment_end.dot(normal)-d)*normal) - p;

  for (int i = 0; i < 3; ++i)
  {
    Eigen::Vector3d q = points[i];
    Eigen::Vector3d s = points[(i+1)%3] - q;

    double u = ((q-p).cross(r)).norm()/(r.cross(s)).norm();
    double t = ((q-p).cross(s)).norm()/(r.cross(s)).norm();

    if (u > -EPISILON && u < 1+ EPISILON && t > -EPISILON && t < 1+EPISILON)
    {
      Eigen::Vector3d closest_point = q + u*s;
      Eigen::Vector3d mesh_close_point = (line_segment_start*(1-t) + line_segment_end*t);
      Eigen::Vector3d short_distance = mesh_close_point - closest_point;

      if (short_distance.norm() < shortest_dist.norm())
      {
        shortest_dist = short_distance;
        mesh_closest_point = mesh_close_point;
      }
    }

  }
  return shortest_dist;
}