Esempio n. 1
0
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();
}
Esempio n. 2
0
// 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();
}
Esempio n. 3
0
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);
}
Esempio n. 4
0
 // 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();
 }
Esempio n. 5
0
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();
}
Esempio n. 6
0
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));
                }
            }
        }
    }
}
Esempio n. 8
0
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;

}
Esempio n. 9
0
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;
}
Esempio n. 10
0
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();
  }
Esempio n. 12
0
/*!
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);
}
Esempio n. 13
0
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 ();
  }
}
Esempio n. 14
0
/*
 * @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;
}
Esempio n. 16
0
 PlanarRangeVis2(Pose plane_pose, LegController<n_joints>* controller, double samples_per_unit, double max_range) : controller(controller), point(point), normal(normal) {
   double d_r = 1.0/samples_per_unit;
   Eigen::Vector3d center = (point.dot(normal) / normal.squaredNorm()) * normal;
   /*
   double yaw = atan2(normal[1], normal[0]);
   double pitch = atan2(normal[2], sqrt(normal[0]*normal[0] + normal[1]*normal[1]));
   center_pose = Pose(center[0], center[1], center[2], yaw, pitch, 0);
   */
   center_pose = plane_pose;
   center_pose.x = center[0];
   center_pose.y = center[1];
   center_pose.z = center[2];
   double angles[n_joints];
   for (double x = -max_range; x < max_range; x += d_r) {
     for (double y = -max_range; y < max_range; y += d_r) {
       Eigen::Vector3d goal = Vector4dTo3d(center_pose.FromFrame(Eigen::Vector4d(0.0, x, y, 1.0)));
       plane_query_scores.push_back(controller->GetJointCommands(goal, angles));
       plane_query.push_back(goal); 
     }
   }
 }
Esempio n. 17
0
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;
}
Esempio n. 18
0
void drawWorldPretty(const World& world,
	const Camera& camera, 
	const VisSettings& settings, 
	SDL_Window* window){

  glEnable(GL_DEPTH_TEST);
  glFrontFace(GL_CCW);
  glEnable(GL_CULL_FACE);
  glClearColor(0.2, 0.2, 0.2, 1);
  glClear(GL_COLOR_BUFFER_BIT| GL_DEPTH_BUFFER_BIT);

  glMatrixMode(GL_PROJECTION);
  glLoadIdentity();

  int windowWidth, windowHeight;
  SDL_GetWindowSize(window, &windowWidth, &windowHeight);
  gluPerspective(45,static_cast<double>(windowWidth)/windowHeight,
	  .5, 100);

  
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();
  gluLookAt(camera.position.x(), camera.position.y(), camera.position.z(),
	  camera.lookAt.x(), camera.lookAt.y(), camera.lookAt.z(),
	  camera.up.x(), camera.up.y(), camera.up.z());

  glEnable (GL_BLEND);
  glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  //avoid typing world.clusters, world.particles everywhere
  const auto& clusters = world.clusters;
  const auto& particles = world.particles;
  const int which_cluster = settings.which_cluster;

  if(!particles.empty()){						
	glPointSize(10);

	if (!settings.drawColoredParticles) {
	  if (settings.which_cluster == -1) {
		glColor4d(1,1,1,0.95);
	  } else {
		glColor4d(1,1,1,0.2);
	  }
	  glEnableClientState(GL_VERTEX_ARRAY);
	  glVertexPointer(3, GL_DOUBLE,
		  sizeof(Particle),
		  &(particles[0].position));
	  glDrawArrays(GL_POINTS, 0, particles.size());
	} else {
	  glBegin(GL_POINTS);
	  for(auto& p : particles) {
		//find nearest cluster

		int min_cluster = p.clusters[0];
		auto com = clusters[min_cluster].worldCom;//computeNeighborhoodCOM(clusters[min_cluster]);
		Eigen::Vector3d dir = p.position - com;
		double min_sqdist = dir.squaredNorm();
		for (auto& cInd : p.clusters) {
		  com = clusters[cInd].worldCom;//computeNeighborhoodCOM(clusters[cInd]);
		  dir = p.position - com;
		  double dist = dir.squaredNorm();
		  if (dist < min_sqdist) {
			min_sqdist = dist;
			min_cluster = cInd;
		  }
		}

		RGBColor rgb = HSLColor(2.0*acos(-1)*(min_cluster%12)/12.0, 0.7, 0.7).to_rgb();
		//RGBColor rgb = HSLColor(2.0*acos(-1)*min_cluster/clusters.size(), 0.7, 0.7).to_rgb();
		if ((which_cluster == -1 || min_cluster == which_cluster) &&
			clusters[min_cluster].members.size() > 1) {
		  //      sqrt(min_sqdist) < 0.55*clusters[min_cluster].renderWidth) {
		  glColor4d(rgb.r, rgb.g, rgb.b, 0.95);
		} else {
		  glColor4d(1,1,1,0.95);
		}
		//glPushMatrix();
		//glTranslated(p.position[0], p.position[1], p.position[2]);
		//utils::drawSphere(0.01, 4, 4);
		glVertex3dv(p.position.data());
		//glPopMatrix();
	  }
	  glEnd();
	}

	/*
	  glPointSize(3);
	  glColor3f(0,0,1);
	  glVertexPointer(3, GL_DOUBLE, sizeof(Particle),
	  &(particles[0].goalPosition));
	  glDrawArrays(GL_POINTS, 0, particles.size());
	*/
  }


  drawPlanesPretty(world.planes, 
	  world.movingPlanes, 
	  world.twistingPlanes, 
	  world.tiltingPlanes, 
	  world.elapsedTime);
  
  auto max_t = std::max_element(
	  clusters.begin(), clusters.end(),
	  [](const Cluster& a, const Cluster& b){
		return a.toughness < b.toughness;
	  })->toughness;
  
  glEnable(GL_POLYGON_OFFSET_FILL);
  glPolygonOffset(1.0,1.0);
  
  //draw cluster spheres
  if(settings.drawClusters){
	glEnable(GL_CULL_FACE);
	glCullFace(GL_BACK);
	glMatrixMode(GL_MODELVIEW);
	for(auto&& pr : benlib::enumerate(clusters)){
	  auto& c = pr.second;
	  const auto i = pr.first;
	  if (which_cluster == -1 || i == which_cluster) {
		glPushMatrix();
			//logic:
			//c.cg.c = original COM of cluster
			//c.worldCom = current COM in world space
			//c.restCom = current COM in rest space
			//c.worldCom - c.restCom = translation of center of mass rest to world
			//trans = location of c in world.
			//auto trans = cg.c + c.worldCom - c.restCom;
			//auto trans = c.worldCom - c.restCom;
			//glTranslated(trans(0), trans(1), trans(2));
			/*
			//step 3, translate from rest space origin to world 
			glTranslated(c.worldCom(0), c.worldCom(1), c.worldCom(2)); 

			//step 2, rotate about rest-to-world transform
			Eigen::Matrix4d gl_rot = Eigen::Matrix4d::Identity();
			//push the rotation onto a 4x4 glMatrix
			//gl_rot.block<3,3>(0,0) << c.restToWorldTransform;
			auto polar = utils::polarDecomp(c.restToWorldTransform);
			gl_rot.block<3,3>(0,0) << polar.first;
			glMultMatrixd(gl_rot.data());

			//step 1, translate rest com to origin
			glTranslated(-c.restCom(0), -c.restCom(1), -c.restCom(2));
			*/

		Eigen::Matrix4d vis_t = c.getVisTransform();
		glMultMatrixd(vis_t.data());
		
		RGBColor rgb = HSLColor(2.0*acos(-1)*(i%12)/12.0, 0.7, 0.7).to_rgb();
		//RGBColor rgb = HSLColor(2.0*acos(-1)*i/clusters.size(), 0.7, 0.7).to_rgb();
		if (settings.colorByToughness) {
		  if (c.toughness == std::numeric_limits<double>::infinity()) {
			rgb = RGBColor(0.0, 0.0, 0.0);
		  } else {
			auto factor = c.toughness/max_t;
			rgb = RGBColor(1.0-factor, factor, factor);
		  }
		}
		glPolygonMode(GL_FRONT,GL_FILL);
		glColor4d(rgb.r, rgb.g, rgb.b, 0.6);
		//utils::drawSphere(c.cg.r, 10, 10);
		utils::drawClippedSphere(c.cg.r, 30, 30, c.cg.c, c.cg.planes);
		glPolygonMode(GL_FRONT,GL_LINE);
		glColor3d(0,0,0);
		utils::drawClippedSphere(c.cg.r, 30, 30, c.cg.c, c.cg.planes);
		glPopMatrix();
	  }
	}
	glDisable(GL_CULL_FACE);
  }
  
  //draw fracture planes 
  glMatrixMode(GL_MODELVIEW);
  if(settings.drawFracturePlanes){
	//glEnable(GL_CULL_FACE);
	//glCullFace(GL_BACK);
	for(auto&& pr : benlib::enumerate(clusters)){
	  auto& c = pr.second;
	  const auto i = pr.first;
	  if (which_cluster == -1 || i == which_cluster) {
		glPushMatrix();
		
		auto& cg = c.cg;
		
		if (cg.planes.size() > 0) {
		  //step 3, translate from rest space origin to world 
		  /*
			glTranslated(c.worldCom(0), c.worldCom(1), c.worldCom(2)); 
			
			//step 2, rotate about rest-to-world transform
			Eigen::Matrix4d gl_rot = Eigen::Matrix4d::Identity();
			//push the rotation onto a 4x4 glMatrix
			//gl_rot.block<3,3>(0,0) << c.restToWorldTransform;
			auto polar = utils::polarDecomp(c.restToWorldTransform);
			gl_rot.block<3,3>(0,0) << polar.first;
			glMultMatrixd(gl_rot.data());
			
			//step 1, translate rest com to origin
			glTranslated(-c.restCom(0), -c.restCom(1), -c.restCom(2));
		  */
		  
		  if (settings.joshDebugFlag) {
			Eigen::Matrix4d vis_t = c.getVisTransform();
			glMultMatrixd(vis_t.data());
			
			RGBColor rgb = HSLColor(2.0*acos(-1)*(i%12)/12.0, 0.7, 0.7).to_rgb();
			for (auto &p : cg.planes) {
			  glPolygonMode(GL_FRONT, GL_FILL);
			  glColor4d(rgb.r, rgb.g, rgb.b, 0.5);
			  //if rest com translated to origin (step 1 above) 
			  ////then we project from c.cg.c to make support point
			  utils::drawPlane(p.first, p.second, c.cg.r, c.cg.c);
			  glPolygonMode(GL_FRONT, GL_LINE);
			  glColor3d(0,0,0);
			  utils::drawPlane(p.first, p.second, c.cg.r, c.cg.c);
			  //if rest com translated to origin (step 1 above) 
			}
		  } else {
			Eigen::Matrix4d vis_t = c.getVisTransform();
			//glMultMatrixd(vis_t.data());
			
			RGBColor rgb = HSLColor(2.0*acos(-1)*(i%12)/12.0, 0.7, 0.7).to_rgb();
			for (auto &p : cg.planes) {
			  glPolygonMode(GL_FRONT, GL_FILL);
			  glColor4d(rgb.r, rgb.g, rgb.b, 0.5);
			  //if rest com translated to origin (step 1 above) 
			  ////then we project from c.cg.c to make support point

			  Eigen::Vector4d norm4(p.first(0), p.first(1), p.first(2), 0);
			  Eigen::Vector4d pt4(c.cg.c(0), c.cg.c(1), c.cg.c(2), 1);
			  double d = norm4.dot(pt4) + p.second;
			  pt4 = pt4 - d*norm4;
			  
			  norm4 = vis_t * norm4;
			  pt4 = vis_t * pt4;
			  
			  Eigen::Vector3d norm3(norm4(0), norm4(1), norm4(2));
			  Eigen::Vector3d pt3(pt4(0), pt4(1), pt4(2));
			  d = norm3.dot(pt3);
			  
			  
			  utils::drawPlane(norm3, -d, c.cg.r, pt3);
			}
			
			//only draw the plane outlines with the transform
			glMultMatrixd(vis_t.data());
			for (auto &p : cg.planes) {
			  glPolygonMode(GL_FRONT, GL_LINE);
			  glColor3d(0,0,0);
			  utils::drawPlane(p.first, p.second, c.cg.r, c.cg.c);
			  //if rest com translated to origin (step 1 above) 
			}
		  }
		}
		glPopMatrix();
	  }
	}
	//glDisable(GL_CULL_FACE);
  }
  
  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
  
  if (settings.which_cluster != -1) {
	const auto& c = clusters[which_cluster];
	
	Eigen::Matrix3d Apq = world.computeApq(c);
	Eigen::Matrix3d A = Apq*c.aInv;
	if (world.nu > 0.0) A = A*c.Fp.inverse(); // plasticity
	
	//do the SVD here so we can handle fracture stuff
	Eigen::JacobiSVD<Eigen::Matrix3d> solver(A, 
		Eigen::ComputeFullU | Eigen::ComputeFullV);
	
	Eigen::Matrix3d U = solver.matrixU(), V = solver.matrixV();
	Eigen::Vector3d sigma = solver.singularValues();
	
	std::cout << "sigma: " << sigma << std::endl;
	
	glColor4d(0,0,0, 0.9);
	
	glPointSize(5);
	
	if (!settings.drawColoredParticles) {
	  glBegin(GL_POINTS);
	  for(auto &member : c.members){
		glVertex3dv(particles[member.index].position.data());
	  }
	  glEnd();
	}
  }
  
  
  
  glColor3d(1, 0, 1);
  for(auto& projectile : world.projectiles){
	glMatrixMode(GL_MODELVIEW);
	glPushMatrix();
	auto currentPosition = projectile.start + world.elapsedTime*projectile.velocity;
	glTranslated(currentPosition.x(), currentPosition.y(), currentPosition.z());
	utils::drawSphere(projectile.radius, 10, 10);
	glPopMatrix();
  }
  
  glColor3d(0,1,0);
  for(auto& cylinder : world.cylinders){
	utils::drawCylinder(cylinder.supportPoint, cylinder.normal, cylinder.radius);
  }
  
  glFlush();
  SDL_GL_SwapWindow(window);
}
Esempio n. 19
0
void HoleFinderPrivate::mergeHoles()
{
  // Make copy, clear original, add merged holes back into original
  QVector<Hole> holeCopy (this->holes);
  const int numHoles = this->holes.size();
  this->holes.clear();
  this->holes.reserve(numHoles);

  // If the bit is on, it has not been merged. If off, it has been.
  QBitArray mask (holeCopy.size(), true);

  // Check each pair of unmerged holes. If one contains the other, merge them.
  QVector<Hole*> toMerge;
  toMerge.reserve(256); // Way bigger than we need, but certainly sufficient

  // Temp vars
  Eigen::Vector3d diffVec;

  // "i" indexes the "base" hole
  for (int i = 0; i < numHoles; ++i) {
    if (!mask.testBit(i))
      continue;

    mask.clearBit(i);
    Hole &hole_i = holeCopy[i];

    toMerge.clear();
    toMerge.reserve(256);
    toMerge.push_back(&hole_i);

    // "j" indexes the compared holes
    for (int j = i+1; j < numHoles; ++j) {
      if (!mask.testBit(j))
        continue;

      Hole &hole_j = holeCopy[j];

      diffVec = hole_j.center - hole_i.center;

      // Use the greater of the two radii
      const double rad = (hole_i.radius > hole_j.radius)
          ? hole_i.radius : hole_j.radius;
      const double radSq = rad * rad;

      // Check periodic conditions
      // Convert diffVec to fractional units
      this->cartToFrac(&diffVec);
      // Adjust each component to range [-0.5, 0.5] (shortest representation)
      while (diffVec.x() < -0.5) ++diffVec.x();
      while (diffVec.y() < -0.5) ++diffVec.y();
      while (diffVec.z() < -0.5) ++diffVec.z();
      while (diffVec.x() > 0.5) --diffVec.x();
      while (diffVec.y() > 0.5) --diffVec.y();
      while (diffVec.z() > 0.5) --diffVec.z();
      // Back to cartesian
      this->fracToCart(&diffVec);

      // if j is within i's radius, add "j" to the merge list
      // and mark "j" as merged
      if (fabs(diffVec.x()) > rad ||
          fabs(diffVec.y()) > rad ||
          fabs(diffVec.z()) > rad ||
          fabs(diffVec.squaredNorm()) > radSq)
        continue; // no match

      // match:
      // Reset j's position to account for periodic wrap-around
      hole_j.center = hole_i.center + diffVec;
      mask.clearBit(j);
      toMerge.push_back(&hole_j);
    }

    if (toMerge.size() == 1)
      this->holes.push_back(hole_i);
    else
      this->holes.push_back(reduceHoles(toMerge));
  }
}
void mesh_core::findSphereTouching3Points(
      Eigen::Vector3d& center,
      double& radius,
      const Eigen::Vector3d& a,
      const Eigen::Vector3d& b,
      const Eigen::Vector3d& c)
{
  Eigen::Vector3d ab = b - a;
  Eigen::Vector3d ac = c - a;
  Eigen::Vector3d n = ab.cross(ac);

  double ab_lensq = ab.squaredNorm();
  double ac_lensq = ac.squaredNorm();
  double n_lensq = n.squaredNorm();

  // points are colinear?
  if (n_lensq <= std::numeric_limits<double>::epsilon())
  {
    findSphereTouching3PointsColinear(
          center,
          radius,
          a,
          b,
          c,
          ab_lensq,
          (c - b).squaredNorm(),
          ac_lensq);
    if (g_verbose)
    {
      logInform("findSphereTouching3Points() COLINEAR QQQQ");
      logInform("           a = (%7.3f %7.3f %7.3f)",
        a.x(),
        a.y(),
        a.z());
      logInform("           b = (%7.3f %7.3f %7.3f)",
        b.x(),
        b.y(),
        b.z());
      logInform("           c = (%7.3f %7.3f %7.3f)",
        c.x(),
        c.y(),
        c.z());
      logInform("           s = (%7.3f %7.3f %7.3f) r=%7.3f",
        center.x(),
        center.y(),
        center.z(),
        radius);
    }
    return;
  }

  Eigen::Vector3d rel_center = (ac_lensq * n.cross(ab) +
                                ab_lensq * ac.cross(n)) /
                               (2.0 * n_lensq);
  radius = rel_center.norm();
  center = rel_center + a;

  if (g_verbose)
  {
    logInform("findSphereTouching3Points()");
    logInform("           a = (%7.3f %7.3f %7.3f)",
      a.x(),
      a.y(),
      a.z());
    logInform("           b = (%7.3f %7.3f %7.3f)",
      b.x(),
      b.y(),
      b.z());
    logInform("           c = (%7.3f %7.3f %7.3f)",
      c.x(),
      c.y(),
      c.z());
    logInform("           s = (%7.3f %7.3f %7.3f) r=%7.3f",
      center.x(),
      center.y(),
      center.z(),
      radius);
  }
}
Esempio n. 21
0
int main()
{
	using std::cos;
	using std::fabs;
	using std::sin;

	const Eigen::Vector3d
		guiding_center_start{5 * earth_radius, 0, 0},
		field_at_start{get_earth_dipole(guiding_center_start)};

	const double gyroperiod
		= 2 * M_PI / fabs(proton_charge_mass_ratio) / field_at_start.norm();

	/*cout <<
		"Energy (eV), pitch angle (deg), steps/gyroperiod: "
		"order of convergence, max relative error in energy\n";*/
	for (auto kinetic_energy: {1e4, 1e7}) { // in eV
	for (auto pitch_angle: {90.0, 30.0}) { // V from B, degrees

		const double particle_speed
			= sqrt(2 * kinetic_energy * proton_charge_mass_ratio);

		const Eigen::Vector3d initial_velocity{
			0,
			sin(pitch_angle / 180 * M_PI) * particle_speed,
			cos(pitch_angle / 180 * M_PI) * particle_speed
		};

		const double gyroradius
			= initial_velocity[1]
			/ proton_charge_mass_ratio
			/ field_at_start.norm();

		const Eigen::Vector3d
			offset_from_gc{gyroradius, 0, 0},
			initial_position{guiding_center_start + offset_from_gc};

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

		for (size_t step_divisor = 1; step_divisor <= (1 << 11); step_divisor *= 2) {

			state_t state{{initial_position, initial_velocity}};
			const double time_step = gyroperiod / step_divisor;

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

			double
				nrj_error = 0,
				mom_error = 0;
			for (double time = 0; time < propagation_time; time += time_step) {

				std::tie(
					state[0],
					state[1]
				) = propagate(
					state[0],
					state[1],
					Vector3d{0, 0, 0},
					get_earth_dipole(state[0]),
					proton_charge_mass_ratio,
					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
				);
			}

			/*cout
				<< kinetic_energy << " "
				<< pitch_angle << " "
				<< step_divisor << ": "
				<< nrj_error << " "
				<< mom_error
				<< endl;*/

			if (
				not check_result(
					kinetic_energy,
					pitch_angle,
					step_divisor,
					nrj_error,
					mom_error,
					state[0]
				)
			) {
				std::cerr <<  __FILE__ << " (" << __LINE__ << "): "
					<< "Unexpected failure in propagator."
					<< std::endl;
				return EXIT_FAILURE;
			}
		}
	}}

	return EXIT_SUCCESS;
}
void CTrackerStereoMotionModel::_trackLandmarks( const cv::Mat& p_matImageLEFT,
                                                 const cv::Mat& p_matImageRIGHT,
                                                 const Eigen::Isometry3d& p_matTransformationEstimateWORLDtoLEFT,
                                                 const Eigen::Isometry3d& p_matTransformationEstimateParallelWORLDtoLEFT,
                                                 const CLinearAccelerationIMU& p_vecLinearAcceleration,
                                                 const Eigen::Vector3d& p_vecRotationTotal )
{
    //ds get images into triple channel mats (display only)
    cv::Mat matDisplayLEFT;
    cv::Mat matDisplayRIGHT;

    //ds get images to triple channel for colored display
    cv::cvtColor( p_matImageLEFT, matDisplayLEFT, cv::COLOR_GRAY2BGR );
    cv::cvtColor( p_matImageRIGHT, matDisplayRIGHT, cv::COLOR_GRAY2BGR );

    //ds get clean copies
    const cv::Mat matDisplayLEFTClean( matDisplayLEFT.clone( ) );
    const cv::Mat matDisplayRIGHTClean( matDisplayRIGHT.clone( ) );

    //ds compute motion scaling
    const double dMotionScaling = 1.0+100*p_vecRotationTotal.squaredNorm( );

    //ds refresh landmark states
    m_cMatcher.resetVisibilityActiveLandmarks( );



    //ds initial transformation
    Eigen::Isometry3d matTransformationWORLDtoLEFT( p_matTransformationEstimateWORLDtoLEFT );

    try
    {
        //ds get the optimized pose
        matTransformationWORLDtoLEFT = m_cMatcher.getPoseOptimizedSTEREO( m_uFrameCount, matDisplayLEFT, matDisplayRIGHT, p_matImageLEFT, p_matImageRIGHT, p_matTransformationEstimateWORLDtoLEFT, p_vecRotationTotal, dMotionScaling );
    }
    catch( const CExceptionPoseOptimization& p_cException )
    {
        std::printf( "<CTrackerStereoMotionModel>(_trackLandmarks) pose optimization failed: '%s' trying parallel transform\n", p_cException.what( ) );
        try
        {
            //ds get the optimized pose on constant motion
            matTransformationWORLDtoLEFT = m_cMatcher.getPoseOptimizedSTEREO( m_uFrameCount, matDisplayLEFT, matDisplayRIGHT, p_matImageLEFT, p_matImageRIGHT, p_matTransformationEstimateParallelWORLDtoLEFT, p_vecRotationTotal, dMotionScaling );
        }
        catch( const CExceptionPoseOptimization& p_cException )
        {
            //ds stick to the last estimate and adopt orientation
            std::printf( "<CTrackerStereoMotionModel>(_trackLandmarks) parallel pose optimization failed: '%s'\n", p_cException.what( ) );
            matTransformationWORLDtoLEFT = m_matTransformationWORLDtoLEFTLAST;
            m_uWaitKeyTimeoutMS = 0;
        }
    }



    //ds respective camera transform
    Eigen::Isometry3d matTransformationLEFTtoWORLD( matTransformationWORLDtoLEFT.inverse( ) );

    //ds estimate acceleration in current WORLD frame (necessary to filter gravity)
    const Eigen::Isometry3d matTransformationIMUtoWORLD( matTransformationLEFTtoWORLD*m_pCameraLEFT->m_matTransformationIMUtoCAMERA );
    const CLinearAccelerationWORLD vecLinearAccelerationWORLD( matTransformationIMUtoWORLD.linear( )*p_vecLinearAcceleration );
    const CLinearAccelerationWORLD vecLinearAccelerationWORLDFiltered( CIMUInterpolator::getLinearAccelerationFiltered( vecLinearAccelerationWORLD ) );

    //ds update acceleration reference
    m_vecLinearAccelerationFilteredLAST = matTransformationWORLDtoLEFT.linear( )*vecLinearAccelerationWORLDFiltered;
    CLogger::CLogLinearAcceleration::addEntry( m_uFrameCount, vecLinearAccelerationWORLD, vecLinearAccelerationWORLDFiltered );

    //ds current translation
    m_vecPositionLAST    = m_vecPositionCurrent;
    m_vecPositionCurrent = matTransformationLEFTtoWORLD.translation( );
    CLogger::CLogTrajectory::addEntry( m_uFrameCount, m_vecPositionCurrent, Eigen::Quaterniond( matTransformationLEFTtoWORLD.linear( ) ) );



    //ds set visible landmarks (including landmarks already detected in the pose optimization)
    const std::shared_ptr< const std::vector< const CMeasurementLandmark* > > vecMeasurements( m_cMatcher.getMeasurementsEpipolar( matDisplayLEFT, matDisplayRIGHT, m_uFrameCount, p_matImageLEFT, p_matImageRIGHT, matTransformationWORLDtoLEFT, matTransformationLEFTtoWORLD, p_vecRotationTotal, dMotionScaling ) );

    //ds compute landmark lost since last (negative if we see more landmarks than before)
    const UIDLandmark uNumberOfVisibleLandmarks = vecMeasurements->size( );
    const int32_t iLandmarksLost                = m_uNumberofVisibleLandmarksLAST-uNumberOfVisibleLandmarks;

    //ds if we lose more than 75% landmarks in one frame
    if( 0.75 < static_cast< double >( iLandmarksLost )/m_uNumberofVisibleLandmarksLAST )
    {
        std::printf( "<CTrackerStereoMotionModel>(_trackLandmarks) lost track (landmarks lost: %i), total delta: %f (%f %f %f)\n", iLandmarksLost, m_vecVelocityAngularFilteredLAST.squaredNorm( ), m_vecVelocityAngularFilteredLAST.x( ), m_vecVelocityAngularFilteredLAST.y( ), m_vecVelocityAngularFilteredLAST.z( ) );
        //m_uWaitKeyTimeoutMS = 0;
    }

    //ds update reference
    m_uNumberofVisibleLandmarksLAST = uNumberOfVisibleLandmarks;

    //ds debug
    for( const CMeasurementLandmark* pMeasurement: *vecMeasurements )
    {
        //ds compute green brightness based on depth (further away -> darker)
        uint8_t uGreenValue = 255-std::sqrt( pMeasurement->vecPointXYZLEFT.z( ) )*20;
        cv::circle( matDisplayLEFT, pMeasurement->ptUVLEFT, 2, CColorCodeBGR( 0, uGreenValue, 0 ), -1 );
        cv::circle( matDisplayRIGHT, pMeasurement->ptUVRIGHT, 2, CColorCodeBGR( 0, uGreenValue, 0 ), -1 );

        //ds get 3d position in current camera frame
        const CPoint3DCAMERA vecXYZLEFT( matTransformationWORLDtoLEFT*pMeasurement->vecPointXYZWORLDOptimized );

        //ds also draw reprojections
        cv::circle( matDisplayLEFT, m_pCameraLEFT->getProjection( vecXYZLEFT ), 6, CColorCodeBGR( 0, uGreenValue, 0 ), 1 );
        cv::circle( matDisplayRIGHT, m_pCameraRIGHT->getProjection( vecXYZLEFT ), 6, CColorCodeBGR( 0, uGreenValue, 0 ), 1 );
    }

    //ds accumulate orientation
    m_vecCameraOrientationAccumulated += p_vecRotationTotal;

    //ds position delta
    const Eigen::Vector3d vecDelta( m_vecPositionCurrent-m_vecPositionKeyFrameLAST );

    //ds get norm
    m_dTranslationDeltaSquaredNormCurrent = vecDelta.squaredNorm( );

    bool bIsKeyFrameRegular = false;
    bool bIsKeyFrameSliding = false;

    //ds add a keyframe if translational delta is sufficiently high
    if( m_dTranslationDeltaForKeyFrameMetersL2 < m_dTranslationDeltaSquaredNormCurrent           ||
        m_dAngleDeltaForOptimizationRadiansL2 < m_vecCameraOrientationAccumulated.squaredNorm( ) )
    {
        bIsKeyFrameRegular = true;
    }

    //ds check the sliding window
    else
    {
        //ds every nth frame is handled with the sliding window
        if( 0 == m_uFrameCount%m_uFrameToWindowRatio )
        {
            //ds get average position of current and last (ROBUSTNESS)
            const CPoint3DWORLD vecPositionAverage( ( m_vecPositionCurrent+m_vecPositionLAST )/2.0 );
            m_arrFramesSlidingWindow[m_uIndexSlidingWindow] = std::make_pair( vecPositionAverage, new CKeyFrame( 0, matTransformationLEFTtoWORLD, p_vecLinearAcceleration.normalized( ), *vecMeasurements, m_uFrameCount, 0 ) );

            //ds evaluate sliding window
            bIsKeyFrameSliding = _containsSlidingWindowKeyFrame( );

            //ds current slide index
            ++m_uIndexSlidingWindow;
        }
    }

    //ds check if set
    if( bIsKeyFrameRegular || bIsKeyFrameSliding )
    {
        //ds register to matcher
        m_cMatcher.setKeyFrameToVisibleLandmarks( );

        //ds current "id"
        const UIDKeyFrame iIDKeyFrameCurrent = m_vecKeyFrames->size( );

        //ds add the keyframe depending on selection mode
        if( bIsKeyFrameRegular )
        {
            //ds create new frame
            m_vecKeyFrames->push_back( new CKeyFrame( iIDKeyFrameCurrent,
                                                      matTransformationLEFTtoWORLD,
                                                      p_vecLinearAcceleration.normalized( ),
                                                      *vecMeasurements,
                                                      m_uFrameCount,
                                                      _getLoopClosureKeyFrame( iIDKeyFrameCurrent, matTransformationLEFTtoWORLD, matDisplayLEFT ) ) );

            //ds check if optimization is required
            if( m_uIDDeltaKeyFrameForOptimization < iIDKeyFrameCurrent-m_uIDProcessedKeyFrameLAST )
            {
                //ds optimize the segment
                m_cOptimizer.optimizeContinuous( m_uIDProcessedKeyFrameLAST, iIDKeyFrameCurrent );

                assert( m_vecKeyFrames->back( )->bIsOptimized );

                //ds update transformations with optimized ones
                m_uIDProcessedKeyFrameLAST         = iIDKeyFrameCurrent+1;
                matTransformationLEFTtoWORLD       = m_vecKeyFrames->back( )->matTransformationLEFTtoWORLD;
                m_vecPositionCurrent               = matTransformationLEFTtoWORLD.translation( );
                matTransformationWORLDtoLEFT       = matTransformationLEFTtoWORLD.inverse( );
                m_matTransformationWORLDtoLEFTLAST = matTransformationWORLDtoLEFT;
                //m_uWaitKeyTimeoutMS = 0;
            }
        }
        else
        {
            //ds add a copy of the center frame of the window (at position 0,1,2,3,[4],5,6,7,8
            m_vecKeyFrames->push_back( new CKeyFrame( iIDKeyFrameCurrent,
                                                      m_arrFramesSlidingWindow[m_uIndexSlidingWindow-5].second,
                                                      _getLoopClosureKeyFrame( iIDKeyFrameCurrent, m_arrFramesSlidingWindow[m_uIndexSlidingWindow-5].second->matTransformationLEFTtoWORLD, matDisplayLEFT ) ) );

            //ds scene viewer
            m_bIsFrameAvailableSlidingWindow = true;
        }

        //ds update references
        m_uIDFrameOfKeyFrameLAST          = m_vecKeyFrames->back( )->uFrame;
        m_vecPositionKeyFrameLAST         = m_vecPositionCurrent;
        m_vecCameraOrientationAccumulated = Eigen::Vector3d::Zero( );

        //ds update scene in viewer
        m_prFrameLEFTtoWORLD = std::pair< bool, Eigen::Isometry3d >( bIsKeyFrameRegular, matTransformationLEFTtoWORLD );
        m_bIsFrameAvailable  = true;
    }
    else
    {
        //ds update scene in viewer: no keyframe
        m_prFrameLEFTtoWORLD = std::pair< bool, Eigen::Isometry3d >( false, matTransformationLEFTtoWORLD );
        m_bIsFrameAvailable  = true;
    }

    //ds check if we have to detect new landmarks
    if( m_uVisibleLandmarksMinimum > m_uNumberofVisibleLandmarksLAST )
    {
        //ds clean the lower display
        cv::hconcat( matDisplayLEFTClean, matDisplayRIGHTClean, m_matDisplayLowerReference );

        //ds detect landmarks
        const std::shared_ptr< std::vector< CLandmark* > > vecNewLandmarks( _getNewLandmarks( m_uFrameCount, m_matDisplayLowerReference, p_matImageLEFT, p_matImageRIGHT, matTransformationWORLDtoLEFT, matTransformationLEFTtoWORLD, p_vecRotationTotal ) );

        //ds all visible in this frame
        m_uNumberofVisibleLandmarksLAST += vecNewLandmarks->size( );

        //ds add to permanent reference holder
        m_vecLandmarks->insert( m_vecLandmarks->end( ), vecNewLandmarks->begin( ), vecNewLandmarks->end( ) );

        //ds add this measurement point to the epipolar matcher
        m_cMatcher.addDetectionPoint( matTransformationLEFTtoWORLD, vecNewLandmarks );
    }

    //ds build display mat
    cv::Mat matDisplayUpper = cv::Mat( m_pCameraSTEREO->m_uPixelHeight, 2*m_pCameraSTEREO->m_uPixelWidth, CV_8UC3 );
    cv::hconcat( matDisplayLEFT, matDisplayRIGHT, matDisplayUpper );
    _drawInfoBox( matDisplayUpper );
    cv::Mat matDisplayComplete = cv::Mat( 2*m_pCameraSTEREO->m_uPixelHeight, 2*m_pCameraSTEREO->m_uPixelWidth, CV_8UC3 );
    cv::vconcat( matDisplayUpper, m_matDisplayLowerReference, matDisplayComplete );

    //ds display
    cv::imshow( "stereo matching", matDisplayComplete );

    //ds if there was a keystroke
    int iLastKeyStroke( cv::waitKey( m_uWaitKeyTimeoutMS ) );
    if( -1 != iLastKeyStroke )
    {
        //ds user input - reset frame rate counting
        m_uFramesCurrentCycle = 0;

        //ds evaluate keystroke
        switch( iLastKeyStroke )
        {
            case CConfigurationOpenCV::KeyStroke::iEscape:
            {
                _shutDown( );
                return;
            }
            case CConfigurationOpenCV::KeyStroke::iBackspace:
            {
                if( 0 < m_uWaitKeyTimeoutMS )
                {
                    //ds switch to stepwise mode
                    m_uWaitKeyTimeoutMS = 0;
                    std::printf( "<CTrackerStereoMotionModel>(_trackLandmarks) switched to stepwise mode\n" );
                }
                else
                {
                    //ds switch to benchmark mode
                    m_uWaitKeyTimeoutMS = 1;
                    std::printf( "<CTrackerStereoMotionModel>(_trackLandmarks) switched back to benchmark mode\n" );
                }
                break;
            }
            default:
            {
                break;
            }
        }
    }
    else
    {
        _updateFrameRateForInfoBox( );
    }

    //ds update references
    ++m_uFrameCount;
    m_matTransformationLEFTLASTtoLEFTNOW = matTransformationWORLDtoLEFT*m_matTransformationWORLDtoLEFTLAST.inverse( );
    m_matTransformationWORLDtoLEFTLAST   = matTransformationWORLDtoLEFT;
}