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;
}
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);
}
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();
}
// 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();
}
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;
}
//==============================================================================
double State::getSagitalCOMDistance()
{
Eigen::Vector3d xAxis = getCOMFrame().linear().col(0); // x-axis
Eigen::Vector3d d = getCOM() - getStanceAnklePosition();
return d.dot(xAxis);
}
/*! \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;
}
//==============================================================================
double State::getCoronalCOMDistance()
{
Eigen::Vector3d yAxis = getCOMFrame().linear().col(2); // z-axis
Eigen::Vector3d d = getCOM() - getStanceAnklePosition();
return d.dot(yAxis);
}
/** 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;
}
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);
}
}
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;
}
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;
}
}
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;
}
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;
}
mesh_core::Plane::Plane(
const Eigen::Vector3d& normal,
const Eigen::Vector3d& point_on_plane)
: normal_(normal.normalized())
{
d_ = -point_on_plane.dot(normal_);
}
/*! 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;
}
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);
}
//==============================================================================
double State::getCoronalCOMVelocity()
{
Eigen::Vector3d yAxis = getCOMFrame().linear().col(2); // z-axis
Eigen::Vector3d v = getCOMVelocity();
return v.dot(yAxis);
}
//==============================================================================
double State::getSagitalCOMVelocity()
{
Eigen::Vector3d xAxis = getCOMFrame().linear().col(0); // x-axis
Eigen::Vector3d v = getCOMVelocity();
return v.dot(xAxis);
}
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;
}
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;
}
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);
}
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;
}
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;
}
//==============================================================================
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();
}
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;
}
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);
}
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);
}
/* 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() );
}
// 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);
}