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

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

force::force(std::shared_ptr<particle> part1, std::shared_ptr<particle> part2, unsigned int fileId, Eigen::Vector3d pos1, Eigen::Vector3d pos2, Eigen::Vector3d val, double volWater, double distCurr, double distCrit) {
  _part1 = part1;
  _part2 = part2;
  
  _volWater = volWater;
  _distCurr = distCurr;
  _distCrit = distCrit;
  
  if (fabs(pos1.norm()/part1->c().norm() - 1.0) > 0.0001 or fabs(pos2.norm()/part2->c().norm() -1.0 ) > 0.0001 ) {
    std::cerr<<"Particle positions in force and particle files are not the same!"<<std::endl;
    std::cerr<<"pid1 "<<part1->id()<<": ("<<pos1[0]<<" "<<pos1[1]<<" "<<pos1[2]<< ") != (" << part1->c()[0]<<" "<<part1->c()[1]<<" "<<part1->c()[2];
    std::cerr<<"pid2 "<<part2->id()<<": ("<<pos2[0]<<" "<<pos2[1]<<" "<<pos2[2]<< ") != (" << part2->c()[0]<<" "<<part2->c()[1]<<" "<<part2->c()[2]<<");   "<<std::endl;
    
    std::cerr<<"dif1 "<<pos1.norm()/part1->c().norm()<<"; dif2 "<<pos2.norm()/part2->c().norm()<<std::endl<<std::endl;
  }
  
  //Updating particle positions as they are more accurate in fstat
  _part1->c(pos1);
  _part2->c(pos2);
  
  
  _cPZ = Eigen::Vector3d::Zero();
  _val = val;
  _fileId = fileId;
  _calculateStressTensor = false;
  _cP = (pos1-pos2)/2.0 + pos1;
  _axisMatrix = _axisMatrix.Zero();
};

void Mesh::computeIntegratedDivergence(Eigen::VectorXd& integratedDivs,
                                       const Eigen::MatrixXd& gradients) const
{
    for (VertexCIter v = vertices.begin(); v != vertices.end(); v++) {
        double integratedDiv = 0.0;
        Eigen::Vector3d p = v->position;
        
        HalfEdgeCIter he = v->he;
        do {
            Eigen::Vector3d gradient = gradients.row(he->face->index);
            
            Eigen::Vector3d p1 = he->next->vertex->position;
            Eigen::Vector3d p2 = he->next->next->vertex->position;
            
            Eigen::Vector3d e1 = p1 - p;
            Eigen::Vector3d e2 = p2 - p;
            Eigen::Vector3d ei = p2 - p1;
            
            double theta1 = acos((-e2).dot(-ei) / (e2.norm() * ei.norm()));
            double cot1 = 1.0 / tan(theta1);
            
            double theta2 = acos((-e1).dot(ei) / (e1.norm() * ei.norm()));
            double cot2 = 1.0 / tan(theta2);
            
            integratedDiv += e1.dot(gradient) * cot1 + e2.dot(gradient) * cot2;
            
            he = he->flip->next;
            
        } while (he != v->he);
        
        integratedDivs[v->index] = 0.5 * integratedDiv;
    }
}

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

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

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

void ManipTool::update_nv(Eigen::Vector3d lv,Eigen::Vector3d& n_hat,Eigen::Vector3d& nv_dot_fb){
    P_bar = Eigen::Matrix3d::Identity()-n_hat*n_hat.transpose();
    nv_dot = -1*Gama_n*P_bar*L_n*n_hat;
    nv_dot_fb = nv_dot;
    L_n_dot = -beta_n*L_n+(1/(1+pow(lv.norm(),2)))*lv*lv.transpose();
    n_hat = n_hat+nv_dot;
    n_hat = n_hat/n_hat.norm();
    L_n = L_n + L_n_dot;

}

        /** 
         * @brief set pose based on compact scaled-axis representation
         *
         * @param v compact 6 vector [r t], where r is a 3 vector representing
         *  the rotation in scaled axis form, and t is the translation 3 vector.
         */
        void fromVector6d( const Vector6d &v )
        {
            const Eigen::Vector3d saxis = v.head<3>();
            if( saxis.norm() > 1e-9 )
            orientation = Eigen::AngleAxisd( saxis.norm(), saxis.normalized() );
            else
            orientation = Eigen::Quaterniond::Identity();

            position = v.tail<3>();
        }

Eigen::Vector3d Triangle::shortestDistanceTo(Triangle* other, Eigen::Vector3d& closest_point){

  Eigen::Vector3d distance = shortestDistanceTo(other->points[0]);
  closest_point = distance + other->points[0];

  //project other triangle vertices onto us
  for (int i = 1; i < 3; ++i)
  {
    Eigen::Vector3d new_dist = shortestDistanceTo(other->points[i]);
    if (new_dist.norm() < distance.norm())
    {
      distance = new_dist;
      closest_point = distance + other->points[i];
    }
  }
  
  //project all vertices onto other triangle
  for (int i = 0; i < 3; ++i)
  {
    Eigen::Vector3d new_dist = -other->shortestDistanceTo(points[i]);
    if (new_dist.norm() < distance.norm())
    {
      distance = new_dist;
      closest_point = points[i];
    }
  }
  
  //project edges onto other triangle, do line intersections

  for (int i = 0; i < 3; ++i)
  {
    Eigen::Vector3d close_point;
    Eigen::Vector3d new_dist = -shortestDistanceTo(other->points[i], other->points[(i+1)%3], close_point);
    if (new_dist.norm() < distance.norm())
    {
      distance = new_dist;
      closest_point = close_point;
    }
  }

  for (int i = 0; i < 3; ++i)
  {
    Eigen::Vector3d close_point;
    Eigen::Vector3d new_dist = other->shortestDistanceTo(points[i],points[(i+1)%3], close_point);
    if (new_dist.norm() < distance.norm())
    {
      distance = new_dist;
      closest_point = points[i];
    }
  }

  return distance;
}

void randomize_transform(Eigen::Isometry3d& tf,
                         double translation_limit=100,
                         double rotation_limit=100)
{
  Eigen::Vector3d r = random_vec<3>(translation_limit);
  Eigen::Vector3d theta = random_vec<3>(rotation_limit);

  tf.setIdentity();
  tf.translate(r);

  if(theta.norm()>0)
    tf.rotate(Eigen::AngleAxisd(theta.norm(), theta.normalized()));
}

// -------------------------------------------------------------------------------
Eigen::Vector3d* ComputeTrixelMidpoints(trixel* trixel)
{
    static Eigen::Vector3d tmp;
    Eigen::Vector3d* midPoints = new Eigen::Vector3d[3];

    tmp = trixel->_vertices[1] + trixel->_vertices[2];
    midPoints[0] = tmp / tmp.norm();
    tmp = trixel->_vertices[0] + trixel->_vertices[2];
    midPoints[1] = tmp / tmp.norm();
    tmp = trixel->_vertices[0] + trixel->_vertices[1];
    midPoints[2] = tmp / tmp.norm();

    return midPoints;
}

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

Quaternion::Quaternion(const Eigen::Vector3d& axis, double angle)
{
  if(axis.norm() == 0.0)
  {
    throw Exception("Quaternion::Quaternion", "Norm of axis is zero.");
  }

  double inv_norm = 1.0 / axis.norm();
  double sin_val = sin(0.5 * angle);

  x_ = axis.coeff(0) * inv_norm * sin_val;
  y_ = axis.coeff(1) * inv_norm * sin_val;
  z_ = axis.coeff(2) * inv_norm * sin_val;
  w_ = cos(0.5 * angle);
}

int main( )
{
    // Use Boost library to make a random number generator.
    boost::mt19937 randomNumbergenerator( time( 0 ) );
    boost::random::uniform_real_distribution< > uniformDistribution( 0.0, 10.0 );
    boost::variate_generator< boost::mt19937&, boost::random::uniform_real_distribution < > >
            generateRandomNumbers( randomNumbergenerator, uniformDistribution );

    // Generate random altitude value between 0 and 10 km.
    const double altitudeKilometers = generateRandomNumbers( );

    // Use the Tudat Core library to convert km to m.
    const double altitudeMeters = tudat::unit_conversions::convertKilometersToMeters(
            altitudeKilometers );

    // Use the Eigen library to create position vectors.
    Eigen::Vector3d positionOfBodySubjectToAcceleration;
    positionOfBodySubjectToAcceleration << 1737.1e3 + altitudeMeters, 0.0, 0.0;
    const Eigen::Vector3d positionOfBodyExertingAcceleration = Eigen::Vector3d::Zero( );

    // Use the Tudat library to compute the acceleration vector.
    const Eigen::Vector3d gravitationalAcceleration =
            tudat::gravitation::computeGravitationalAcceleration(
                    positionOfBodySubjectToAcceleration, 4.9e12,
                    positionOfBodyExertingAcceleration );

    // Print the altitude and norm of the acceleration vector.
    std::cout << "Hello world!" << std::endl;
    std::cout << "I am floating " << altitudeKilometers << " km above the Moon's surface." <<
            std::endl;
    std::cout << "The gravitational acceleration here is " <<
            gravitationalAcceleration.norm() << " m/s^2."  << std::endl;

    return 0;
}

  Eigen::Vector3d R2AxisAngle(Eigen::Matrix3d const & C){
    // Sometimes, because of roundoff error, the value of tr ends up outside
    // the valid range of arccos. Truncate to the valid range.
    double tr = std::max(-1.0, std::min( (C(0,0) + C(1,1) + C(2,2) - 1.0) * 0.5, 1.0));
    double a = acos( tr ) ;

    Eigen::Vector3d axis;

    if(fabs(a) < 1e-10) {
      return Eigen::Vector3d::Zero();
    }
		
    axis[0] = (C(2,1) - C(1,2));
    axis[1] = (C(0,2) - C(2,0));
    axis[2] = (C(1,0) - C(0,1));
    double n2 = axis.norm();
    if(fabs(n2) < 1e-10)
      return Eigen::Vector3d::Zero();
		
    double scale = -a/n2;
    axis = scale * axis;

    return axis;

  }