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

void HoleFinder::setTranslations(const Eigen::Vector3d &v1,
                                 const Eigen::Vector3d &v2,
                                 const Eigen::Vector3d &v3)
{
  Q_D(HoleFinder);
  d->v1 = v1;
  d->v2 = v2;
  d->v3 = v3;
  d->cartMat.col(0) = v1;
  d->cartMat.col(1) = v2;
  d->cartMat.col(2) = v3;
  d->fracMat = d->cartMat.inverse();
  double v1SqrNorm = v1.squaredNorm();
  double v2SqrNorm = v2.squaredNorm();
  double v3SqrNorm = v3.squaredNorm();
  d->v1Norm = sqrt(v1SqrNorm);
  d->v2Norm = sqrt(v2SqrNorm);
  d->v3Norm = sqrt(v3SqrNorm);
  d->cellVolume = fabs(v1.dot(v2.cross(v3)));

  DDEBUGOUT("setTranslations")
      QString("Cart matrix:\n %L1 %L2 %L3\n %L4 %L5 %L6\n %L7 %L8 %L9")
      .arg(d->cartMat(0,0), 10)
      .arg(d->cartMat(0,1), 10)
      .arg(d->cartMat(0,2), 10)
      .arg(d->cartMat(1,0), 10)
      .arg(d->cartMat(1,1), 10)
      .arg(d->cartMat(1,2), 10)
      .arg(d->cartMat(2,0), 10)
      .arg(d->cartMat(2,1), 10)
      .arg(d->cartMat(2,2), 10);
  DDEBUGOUT("setTranslations")
      QString("Frac matrix:\n %L1 %L2 %L3\n %L4 %L5 %L6\n %L7 %L8 %L9")
      .arg(d->fracMat(0,0), 10)
      .arg(d->fracMat(0,1), 10)
      .arg(d->fracMat(0,2), 10)
      .arg(d->fracMat(1,0), 10)
      .arg(d->fracMat(1,1), 10)
      .arg(d->fracMat(1,2), 10)
      .arg(d->fracMat(2,0), 10)
      .arg(d->fracMat(2,1), 10)
      .arg(d->fracMat(2,2), 10);

  int numCells = 27;
  d->points.reserve(numCells * d->numPoints);

  DDEBUGOUT("setTranslations") "VectorLengths:"
      << d->v1Norm  << d->v2Norm  << d->v3Norm;
  DDEBUGOUT("setTranslations") "CellVolume" << d->cellVolume;
  DDEBUGOUT("setTranslations") "NumPoints:" << d->numPoints;
  DDEBUGOUT("setTranslations") "NumCells:" << numCells;
  DDEBUGOUT("setTranslations") "Allocated points:" << d->points.capacity();
}

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

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

 // converts compact quaternion (3D vector part of quaternion) to a rotation matrix
 inline Eigen::Matrix3d fromCompactQuat(const Eigen::Vector3d& v) {
   double w = 1-v.squaredNorm();
   if (w<0)
     return Eigen::Matrix3d::Identity();
   else
     w=sqrt(w);
   return Eigen::Quaterniond(w, v[0], v[1], v[2]).toRotationMatrix();
 }

void CylindricalNVecConstr::impl_compute(result_t& res, const argument_t& x) const
{
  pgdata_->x(x);

  sva::PTransformd pos = surfaceFrame_*pgdata_->mbc().bodyPosW[bodyIndex_];
  Eigen::Vector3d vec = (targetFrame_.translation() - pos.translation());
  double dot = vec.dot(pos.rotation().row(2));
  double sign = std::copysign(1., dot);
  res(0) = sign*std::pow(dot, 2) - vec.squaredNorm();
}

bool VSOArray::pointTelescopes(const Eigen::Vector3d& v)
{
  if(v.squaredNorm() == 0)
    return false;

  bool good = true;
  for(std::vector<VSOTelescope*>::iterator i = fTelescopes.begin();
      i!=fTelescopes.end(); i++)good &= (*i)->pointTelescope(v);

  return good;
}

void NormalModeMeanSquareFluctuationCalculator::calculate_hessian_matrix(
        const Protein& protein,
        const ForceConstantSelector & kk_selector,
        const std::shared_ptr<ProteinSegment> &protein_segment) {
    LOGD << "calculating hessian matrix";
        Eigen::VectorXd ave = protein_segment->displacement_vector();

    for (size_t ires=0; ires < this->residue_count; ++ires) {
        for (size_t jres=0; jres < ires; ++jres) {
            Eigen::Vector3d dr = ave.segment<3>(3 * ires) - ave.segment<3>(3 * jres);

            double range1_2 = dr.squaredNorm();

            double kk = kk_selector.select_force_constant(
                    protein.get_secondary_structure_type_of_atom_pair(ires + 1, jres + 1),
                    ires - jres, range1_2);

            // calnmodemfs.f:72
            for (size_t ixyz = 0; ixyz < 3; ixyz++) {
                for (size_t jxyz = 0; jxyz < 3; jxyz++) {
                    size_t i = 3 * ires + ixyz;
                    size_t j = 3 * ires + jxyz;

                    this->hessian_matrix(i, j) += kk * dr(ixyz) * dr(jxyz) / range1_2;
                    i = 3 * ires + ixyz;
                    j = 3 * jres + jxyz;
                    this->hessian_matrix(i, j) -= kk * dr(ixyz) * dr(jxyz) / range1_2;
                    i = 3 * jres + ixyz;
                    j = 3 * ires + jxyz;
                    this->hessian_matrix(i, j) -= kk * dr(ixyz) * dr(jxyz) / range1_2;
                    i = 3 * jres + ixyz;
                    j = 3 * jres + jxyz;
                    this->hessian_matrix(i, j) += kk * dr(ixyz) * dr(jxyz) / range1_2;
                }
            }
        }
    }

    // mass weighted
    // calmodemsfss.f:200
    for (size_t ires = 0; ires < this->residue_count; ++ires) {
        for (size_t ixyz = 0; ixyz < 3; ++ixyz) {
            size_t i = 3 * ires+ixyz;
            for (size_t jres = 0; jres < this->residue_count; ++jres) {
                for (size_t jxyz = 0; jxyz < 3; ++jxyz) {
                    size_t j = 3 * jres + jxyz;
                    this->hessian_matrix(i,j) /= sqrt(mass(ires) * mass(jres));
                }
            }
        }
    }
}

int collideSphereSphere(CollisionObject* o1, CollisionObject* o2, const double& _r0, const Eigen::Isometry3d& c0,
                        const double& _r1, const Eigen::Isometry3d& c1,
                        CollisionResult& result)
{
  double r0 = _r0;
  double r1 = _r1;
  double rsum = r0 + r1;
  Eigen::Vector3d normal = c0.translation() - c1.translation();
  double normal_sqr = normal.squaredNorm();

  if ( normal_sqr > rsum * rsum )
  {
    return 0;
  }

  r0 /= rsum;
  r1 /= rsum;

  Eigen::Vector3d point = r1 * c0.translation() + r0 * c1.translation();
  double penetration;

  if (normal_sqr < DART_COLLISION_EPS)
  {
    normal.setZero();
    penetration = rsum;

    Contact contact;
    contact.collisionObject1 = o1;
    contact.collisionObject2 = o2;
    contact.point = point;
    contact.normal = normal;
    contact.penetrationDepth = penetration;
    result.addContact(contact);
    return 1;
  }

  normal_sqr = sqrt(normal_sqr);
  normal *= (1.0/normal_sqr);
  penetration = rsum - normal_sqr;

  Contact contact;
  contact.collisionObject1 = o1;
  contact.collisionObject2 = o2;
  contact.point = point;
  contact.normal = normal;
  contact.penetrationDepth = penetration;
  result.addContact(contact);
  return 1;

}

Eigen::Vector3d Circuit::GetFieldFromPoint(
  const Eigen::Vector3d& point,
  const Eigen::Vector3d& segmentCenter,
  const Eigen::Vector3d& flowDirection,
  double segmentCoeff
) {
  Eigen::Vector3d r = segmentCenter.array()-point.array();
  double rLengthSquared = r.squaredNorm(); // distance squared.
  Eigen::Vector3d rUnit = r.normalized();
  Eigen::Vector3d Vcrossr = flowDirection.cross(rUnit);

  Eigen::Vector3d B = segmentCoeff/rLengthSquared*Vcrossr.array();
  return B;
}

void IACTDetectorSphereCherenkovPhotonIntersectionFinder::
visit_cherenkov_photon(const calin::simulation::air_cherenkov_tracker::CherenkovPhoton& cherenkov_photon)
{
  calin::math::ray::Ray ray(cherenkov_photon.x0, cherenkov_photon.u0,
    cherenkov_photon.t0, cherenkov_photon.epsilon);
  unsigned scope_id = 0;
  bool do_refraction = do_refraction_;
  double refraction_safety_margin = refraction_safety_margin_;
  for(auto& isphere : visitor_->detector_spheres()) {
    Eigen::Vector3d dx = cherenkov_photon.x0 - isphere.r0;
    double u0_dot_dx = cherenkov_photon.u0.dot(dx);
    double dr2 = dx.squaredNorm() - u0_dot_dx*u0_dot_dx;
    if(dr2 < SQR(isphere.radius + refraction_safety_margin)) {
      // we have a potential hit !
      if(do_refraction) {
        // Here we propagate to the observation plane associated with the
        // detector sphere then refract the ray then back-propagate to the
        // height of the photon emission so absoption can be calculated later
        atm_->propagate_ray_with_refraction(ray, isphere.iobs, /* time_reversal_ok = */ false);
        double reverse_propagate_dist = (cherenkov_photon.x0.z() - ray.z())/ray.uz();
        ray.propagate_dist(reverse_propagate_dist);
        do_refraction = false; // don't refract twice if we have overlapping telescopes
        refraction_safety_margin = 0.0;
        dx = cherenkov_photon.x0 - isphere.r0;
        u0_dot_dx = cherenkov_photon.u0.dot(dx);
        dr2 = dx.squaredNorm() - u0_dot_dx*u0_dot_dx;
        if(dr2<SQR(isphere.radius)) {
          visitor_->process_ray(scope_id, ray, /* pe_weight = */ 1.0);
        }
      } else {
        visitor_->process_ray(scope_id, ray, /* pe_weight = */ 1.0);
      }
    }
    ++scope_id;
  }
}

  void CEViewOptionsWidget::updateMillerPlane()
  {
    // View into a Miller plane
    Camera *camera = m_glWidget->camera();
    Eigen::Transform3d modelView;
    modelView.setIdentity();

    // OK, so we want to rotate to look along the normal at the plane
    // So we convert <h k l> into a Cartesian normal
    Eigen::Matrix3d cellMatrix = m_ext->unconvertLength(m_ext->currentCellMatrix()).transpose();
    // Get miller indices:
    const Eigen::Vector3d millerIndices
      (static_cast<double>(ui.spin_mi_h->value()),
       static_cast<double>(ui.spin_mi_k->value()),
       static_cast<double>(ui.spin_mi_l->value()));
    // Check to see if we have 0,0,0
    //  in which case, we do nothing
    if (millerIndices.squaredNorm() < 0.5)
      return;

    const Eigen::Vector3d normalVector ((cellMatrix * millerIndices).normalized());

    Eigen::Matrix3d rotation;
    rotation.row(2) = normalVector;
    rotation.row(0) = rotation.row(2).unitOrthogonal();
    rotation.row(1) = rotation.row(2).cross(rotation.row(0));

    // Translate camera to the center of the cell
    const Eigen::Vector3d cellDiagonal =
        cellMatrix.col(0) * m_glWidget->aCells() +
        cellMatrix.col(1) * m_glWidget->bCells() +
        cellMatrix.col(2) * m_glWidget->cCells();

    modelView.translate(-cellDiagonal*0.5);

    // Prerotate the camera to look down the specified normal
    modelView.prerotate(rotation);


    // Pretranslate in the negative Z direction
    modelView.pretranslate(Eigen::Vector3d(0.0, 0.0,
                                           -1.5 * cellDiagonal.norm()));

    camera->setModelview(modelView);

    // Call for a redraw
    m_glWidget->update();
  }

/*!
Returns propagated state and max relative errors in
kinetic energy and magnetic moment of particle.
*/
template<class Stepper> std::tuple<state_t, double, double> trace_trajectory(
	state_t state,
	const double time_step
) {
	using std::fabs;
	using std::max;

	constexpr double propagation_time = 424.1; // seconds, 1/100 of doi above

	Particle_Propagator propagator(proton_charge_mass_ratio);

	const double
		initial_energy = 0.5 * proton_mass * state[1].squaredNorm(),
		initial_magnetic_moment
			= proton_mass
			* std::pow(state[1][1], 2)
			/ (2 * get_earth_dipole(state[0]).norm());

	Stepper stepper;
	double nrj_error = 0, mom_error = 0;
	for (double time = 0; time < propagation_time; time += time_step) {
		stepper.do_step(propagator, state, time, time_step);

		const Eigen::Vector3d
			v(state[1]),
			b(get_earth_dipole(state[0])),
			v_perp_b(v - (v.dot(b) / b.squaredNorm()) * b);

		const double
			current_energy
				= 0.5 * proton_mass * state[1].squaredNorm(),
			current_magnetic_moment
				= proton_mass * v_perp_b.squaredNorm() / (2 * b.norm());

		nrj_error = max(
			nrj_error,
			fabs(current_energy - initial_energy) / initial_energy
		);
		mom_error = max(
			mom_error,
			fabs(current_magnetic_moment - initial_magnetic_moment)
				/ initial_magnetic_moment
		);
	}

	return std::make_tuple(state, nrj_error, mom_error);
}

template <typename PointT> void
pcl::SampleConsensusModelCircle3D<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
{
  // Check if the model is valid given the user constraints
  if (!isModelValid (model_coefficients))
  {
    distances.clear ();
    return;
  }
  distances.resize (indices_->size ());

  // Iterate through the 3d points and calculate the distances from them to the sphere
  for (size_t i = 0; i < indices_->size (); ++i)
  // Calculate the distance from the point to the circle:
  // 1.   calculate intersection point of the plane in which the circle lies and the
  //      line from the sample point with the direction of the plane normal (projected point)
  // 2.   calculate the intersection point of the line from the circle center to the projected point
  //      with the circle
  // 3.   calculate distance from corresponding point on the circle to the sample point
  {
    // what i have:
    // P : Sample Point
    Eigen::Vector3d P (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z);
    // C : Circle Center
    Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
    // N : Circle (Plane) Normal
    Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
    // r : Radius
    double r = model_coefficients[3];

    Eigen::Vector3d helper_vectorPC = P - C;
    // 1.1. get line parameter
    double lambda = (helper_vectorPC.dot (N)) / N.squaredNorm ();

    // Projected Point on plane
    Eigen::Vector3d P_proj = P + lambda * N;
    Eigen::Vector3d helper_vectorP_projC = P_proj - C;

    // K : Point on Circle
    Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
    Eigen::Vector3d distanceVector =  P - K;

    distances[i] = distanceVector.norm ();
  }
}

/*
 * @brief Convert the vector part of a quaternion to a
 *    full quaternion.
 * @note This function is useful to convert delta quaternion
 *    which is usually a 3x1 vector to a full quaternion.
 *    For more details, check Equation (238) and (239) in
 *    "Indirect Kalman Filter for 3D Attitude Estimation:
 *    A Tutorial for quaternion Algebra".
 */
inline Eigen::Vector4d smallAngleQuaternion(
    const Eigen::Vector3d& dtheta) {

  Eigen::Vector3d dq = dtheta / 2.0;
  Eigen::Vector4d q;
  double dq_square_norm = dq.squaredNorm();

  if (dq_square_norm <= 1) {
    q.head<3>() = dq;
    q(3) = std::sqrt(1-dq_square_norm);
  } else {
    q.head<3>() = dq;
    q(3) = 1;
    q = q / std::sqrt(1+dq_square_norm);
  }

  return q;
}

int ParticleManagerTinkerToy::findClosestParticleIndex( double x, double y, bool fIgnoreLast )
{
	double distance =  DBL_MAX;
	int closestParticleIndex = -1;
	Eigen::Vector3d positionCurrent( x, y, 0 );
	int lastParticleIndex = fIgnoreLast ? m_particles.size() - 1: m_particles.size();
	for (int i = 0; i < lastParticleIndex; i++) //Ignore last added particle
	{
		Eigen::Vector3d distVector = m_particles[i]->mPosition - positionCurrent;
		double distCur = distVector.squaredNorm();
		if ( distCur < distance )
		{
			distance = distCur;
			closestParticleIndex = i;
		}
	}
	return closestParticleIndex;
}