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;
}
Eigen::Matrix3d Bd970::getOrientationUncertainty(void)
{
Eigen::Matrix3d orientation_uncertainty;
/** Get the yaw with respect to the north **/
Eigen::Vector3d variance;
variance << 0.00, 0.00, (m_current_nmea.data_gst.heading_sigma_error*m_current_nmea.data_gst.heading_sigma_error);
orientation_uncertainty = variance.array().matrix().asDiagonal();
return orientation_uncertainty;
}
void mesh_core::generateAABB(
const EigenSTL::vector_Vector3d& points,
Eigen::Vector3d& min,
Eigen::Vector3d& max)
{
min = Eigen::Vector3d(std::numeric_limits<double>::max(),
std::numeric_limits<double>::max(),
std::numeric_limits<double>::max());
max = Eigen::Vector3d(-std::numeric_limits<double>::max(),
-std::numeric_limits<double>::max(),
-std::numeric_limits<double>::max());
EigenSTL::vector_Vector3d::const_iterator it = points.begin();
EigenSTL::vector_Vector3d::const_iterator end = points.end();
for ( ; it != end ; ++it)
{
min = min.array().min(it->array());
max = max.array().max(it->array());
}
}
Eigen::Vector3d Atom::CalculateElectricalForce(const Eigen::Vector3d& chargePosition, Real charge, shared_ptr<ForceVectors> forceVectorStorage) {
Eigen::Vector3d forceDirection = chargePosition.array()-m_position.array();
Real rSquared = forceDirection.squaredNorm();
Real forceMagnitude = K_VACUUM*charge*m_effectiveNuclearCharge*Square(ELECTRIC_CHARGE)/rSquared;
// The force is in the direction of the charge.
forceDirection.normalize();
Eigen::Vector3d forceVector = forceDirection.array()*forceMagnitude;
if(forceVectorStorage) {
forceVectorStorage->AddForceVector(GetName(), forceVector);
}
// Add in forces due to sp orbitals.
for(int i=0; i<m_spOrbitals.size(); i++) {
shared_ptr<SpOrbital> orbital = m_spOrbitals[i];
Eigen::Vector3d spForce = orbital->CalculateElectricalForce(chargePosition, charge, forceVectorStorage);
forceVector.array() += spForce.array();
}
return forceVector;
}
Eigen::Matrix3d Bd970::getPositionUncertainty(void)
{
Eigen::Matrix3d position_uncertainty;
Eigen::Vector3d eigenvalues; // eigen values are the square of the std deviation
eigenvalues << m_current_nmea.data_gst.semi_major_axis_sigma_error *
m_current_nmea.data_gst.semi_major_axis_sigma_error,
m_current_nmea.data_gst.semi_minor_axis_sigma_error *
m_current_nmea.data_gst.semi_minor_axis_sigma_error,
m_current_nmea.data_gst.height_sigma_error *
m_current_nmea.data_gst.height_sigma_error;
Eigen::Matrix3d U = this->getOrientation().matrix();
position_uncertainty = U * eigenvalues.array().matrix().asDiagonal() * U.transpose();
return position_uncertainty;
}
Eigen::Vector3d Circuit::GetField(const Eigen::Vector3d& point) {
double segmentLength = 2.0*NXGR_PI*m_radius/m_segments;
double segmentCoeff = PERMIABILITY_OVER_4PI*segmentLength*m_current;
Eigen::Vector3d B(0.0);
Eigen::Vector3d pos(0.0, 0.0, 0.0);
Eigen::Vector3d up(0.0, 0.0, 1.0);
double angle = 0.0;
double angleInc = 2.0*NXGR_PI/m_segments;
for(int i=0; i<m_segments; i++) {
pos[0] = m_radius*cos(angle);
pos[1] = m_radius*sin(angle);
Eigen::Vector3d flow = pos.cross(up);
flow.normalize();
Eigen::Vector3d segmentB = GetFieldFromPoint(point, pos, flow, segmentCoeff);
B.array() += segmentB.array();
angle += angleInc;
}
return B;
}
void igl::fit_plane(
const Eigen::MatrixXd & V,
Eigen::RowVector3d & N,
Eigen::RowVector3d & C)
{
assert(V.rows()>0);
Eigen::Vector3d sum = V.colwise().sum();
Eigen::Vector3d center = sum.array()/(double(V.rows()));
C = center;
double sumXX=0.0f,sumXY=0.0f,sumXZ=0.0f;
double sumYY=0.0f,sumYZ=0.0f;
double sumZZ=0.0f;
for(int i=0;i<V.rows();i++)
{
double diffX=V(i,0)-center(0);
double diffY=V(i,1)-center(1);
double diffZ=V(i,2)-center(2);
sumXX+=diffX*diffX;
sumXY+=diffX*diffY;
sumXZ+=diffX*diffZ;
sumYY+=diffY*diffY;
sumYZ+=diffY*diffZ;
sumZZ+=diffZ*diffZ;
}
Eigen::MatrixXd m(3,3);
m << sumXX,sumXY,sumXZ,
sumXY,sumYY,sumYZ,
sumXZ,sumYZ,sumZZ;
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(m);
N = es.eigenvectors().col(0);
}
void SceneCloudView::update_current_cloud(const sensor_msgs::PointCloud2ConstPtr& msg, const Eigen::Affine3d& cam_pose, const Eigen::Vector3d& world_size)
{
if (!cur_view_added_)
return;
pcl17::PointCloud<pcl17::PointXYZ> cloud;
pcl17::fromROSMsg(*msg, cloud);
current_cloud_ptr_->points.clear();
current_cloud_ptr_->width = (int)cloud.points.size();
current_cloud_ptr_->height = 1;
BOOST_FOREACH (const pcl17::PointXYZ& pt, cloud.points)
{
Eigen::Vector3d p(pt.x, pt.y, pt.z);
Eigen::Vector3d g_p = cam_pose * p;// + world_size / 2;
if ((g_p.array() < world_size.array()).all())
{
current_cloud_ptr_->points.push_back(pcl17::PointXYZ(g_p(0), g_p(1), g_p(2)));
}
}
}
