Ejemplo n.º 1
0
void CState::randuVelocity(double mean, double vmax, long *seed)
{
    mat velocities(nAtoms, 3);
    rowvec momentum(nAtoms);

    // generating random, uniform velocities
    for (int i = 0 ; i < nAtoms; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            velocities(i, j) = (ran2(seed)-0.5)*vmax + mean;
        }
    }
    // removing any linear momentum from the system
    momentum = sum(velocities)/nAtoms;
    for(int i = 0; i < nAtoms; i++)
    {
        velocities.row(i) -= momentum;
    }
    // sending the velocities to the atoms
    for (int i = 0; i < nAtoms; i++)
    {
        atoms[i]->setVelocity(velocities.row(i).t());
    }
}
Ejemplo n.º 2
0
void CState::randnVelocity(double mean, double sigma_v, long *seed)
{
    vec2 randomNormal, randomUniform;
    mat velocities(nAtoms, 3);
    rowvec3 momentum;
    double R;

    for (int i = 0; i < nAtoms; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            // Box-Muller transform
            randomUniform << ran2(seed) << ran2(seed);
            R = sqrt(-2*log(randomUniform(0)));
            randomNormal(0) = R*cos(2*pi*randomUniform(1));
            velocities(i, j) = randomNormal(0)*sigma_v + mean;

            // unused random number... should probably use this one for something
            // randomNormal(1) = R*sin(2*pi*randomUniform(1))*sigma_v + mean;
        }
    }

    momentum = sum(velocities)/nAtoms; // finding the linear momentum of the system
    for (int i = 0; i < nAtoms; i++)
    {
        velocities.row(i) -= momentum; // removing initial linear momentum from the system
    }

    // sending the generated velocities to the atoms
    for (int i = 0; i < nAtoms; i++)
    {
        atoms[i]->setVelocity(velocities.row(i).t());
    }
}
Ejemplo n.º 3
0
int hybrid_fmm_stokes_solver(int ac, char **av)
{
    srand(0);
    bool dump_tree_data = false;
    vtkSmartPointer<vtkPolyData>   poly_data = vtkSmartPointer<vtkPolyData>::New();
    vtkSmartPointer<vtkPoints>     points = vtkSmartPointer<vtkPoints>::New();
    vtkSmartPointer<vtkFloatArray> data_points = vtkSmartPointer<vtkFloatArray>::New();
    vtkSmartPointer<vtkCellArray>  cells = vtkSmartPointer<vtkCellArray>::New();
    typedef float value_type;
    size_t num_sources = 1 << 8;
    size_t num_targets = 1 << 8;
    size_t size_sources = 3 * num_sources;
    size_t size_targets = 3 * num_targets;
    std::vector<value_type> sources(size_sources), targets(size_targets), velocities(size_targets), forces(size_sources);
    value_type delta = .0001;
    HybridFmmStokesSolver<value_type> fmm(num_sources);
    std::generate(sources.begin(), sources.end(), random_generator<value_type>());
    std::generate(targets.begin(), targets.end(), random_generator<value_type>());
    std::generate(forces.begin(), forces.end(), random_generator<value_type>());
    std::cout << "sources = ["; std::copy(sources.begin(), sources.end(), std::ostream_iterator<value_type>(std::cout, " ")); std::cout << "];" << std::endl;
    std::cout << "targets = ["; std::copy(targets.begin(), targets.end(), std::ostream_iterator<value_type>(std::cout, " ")); std::cout << "];" << std::endl;
    std::cout << "forces = ["; std::copy(forces.begin(), forces.end(), std::ostream_iterator<value_type>(std::cout, " ")); std::cout << "];" << std::endl;
    fmm.setDelta(delta);
    fmm(0, &sources[0], &velocities[0], &forces[0]);
    std::fill(velocities.begin(), velocities.end(), 0.0);
    fmm.allPairs();
    if (dump_tree_data)
    {
        IO::VtkWriter<vtkXMLPolyDataWriter> writer("data/points");
        data_points->SetArray(&sources[0], sources.size(), 1);
        data_points->SetNumberOfComponents(3);
        data_points->SetName("positions");
        points->SetData(data_points);
        poly_data->SetPoints(points);
        for (vtkIdType i = 0; i < num_sources; ++i)
            cells->InsertNextCell(VTK_VERTEX, &i);
        poly_data->SetVerts(cells);
        writer.write(poly_data, 0, false);
        write_vtk_octree();
    }
#ifdef USE_QT_GUI
    QApplication app(ac, av);

    if (!QGLFormat::hasOpenGL())
    {
        std::cerr << "This system has no OpenGL support" << std::endl;
        return 1;
    }
    QGL::setPreferredPaintEngine(QPaintEngine::OpenGL);
    OctreeRenderer tree_renderer;
    tree_renderer.init(octree);
    tree_renderer.setWindowTitle(QObject::tr("Quad Tree"));
    tree_renderer.setMinimumSize(200, 200);
    tree_renderer.resize(800, 600);
    tree_renderer.show();
    return app.exec();
#else
    return 0;
#endif
}
Ejemplo n.º 4
0
// Cartpole state is: position, velocity, angle, angular velocity;
// for the angle, 0 is straigth up, PI is straigth down
ConsistentVector Cartpole::dynamics(double time,
				    const ConsistentVector& state,
				    const ConsistentVector &control) const {
  checkStateSize(state);
  checkControlSize(control);
  ConsistentVector velocities(state.size());

  double sint = std::sin(state[2]);
  double cost = std::cos(state[2]);

  double g = 9.81;
  double M = param_[CART_MASS];
  double m = param_[POLE_MASS];
  double l = param_[POLE_LENGTH];
  double f = param_[CART_POLE_FRICTION];

  double div = M + (1 - cost*cost)*m;

  velocities[0] = state[1];
  velocities[1] = (-m*g*sint*cost + m*l*state[3]*state[3]*sint + f*m*state[3]*cost + control[0])/div;
  velocities[2] = state[3];
  velocities[3] = ((M + m)*(g*sint - f*state[3]) - (l*m*state[3]*state[3]*sint + control[0])*cost)/(l*div);

  return velocities;
}
int main(){
	
	atoms();
	velocities();
	bonds();
	angles();
	dihedrals();
}
Ejemplo n.º 6
0
void Foam::cfdemCloudIB::calcVelocityCorrection
(
    volScalarField& p,
    volVectorField& U,
    volScalarField& phiIB,
    volScalarField& voidfraction
)
{
    label cellI=0;
    vector uParticle(0,0,0);
    vector rVec(0,0,0);
    vector velRot(0,0,0);
    vector angVel(0,0,0);

    for(int index=0; index< numberOfParticles(); index++)
    {
        //if(regionM().inRegion()[index][0])
        //{
            for(int subCell=0;subCell<voidFractionM().cellsPerParticle()[index][0];subCell++)
            {
                //Info << "subCell=" << subCell << endl;
                cellI = cellIDs()[index][subCell];

                if (cellI >= 0)
                {
                    // calc particle velocity
                    for(int i=0;i<3;i++) rVec[i]=U.mesh().C()[cellI][i]-position(index)[i];
                    for(int i=0;i<3;i++) angVel[i]=angularVelocities()[index][i];
                    velRot=angVel^rVec;
                    for(int i=0;i<3;i++) uParticle[i] = velocities()[index][i]+velRot[i];

                    // impose field velocity
                    U[cellI]=(1-voidfractions_[index][subCell])*uParticle+voidfractions_[index][subCell]*U[cellI];
                }
            }
        //}
    }

    // make field divergence free - set reference value in case it is needed
    fvScalarMatrix phiIBEqn
    (
        fvm::laplacian(phiIB) == fvc::div(U) + fvc::ddt(voidfraction)
    );
     if(phiIB.needReference()) 
     {
         phiIBEqn.setReference(pRefCell_, pRefValue_);
     }
    
    phiIBEqn.solve();

    U=U-fvc::grad(phiIB);
    U.correctBoundaryConditions();

    // correct the pressure as well
    p=p+phiIB/U.mesh().time().deltaT();  // do we have to  account for rho here?
    p.correctBoundaryConditions();
}
Ejemplo n.º 7
0
bool HuboPath::Trajectory::interpolate()
{
    if( elements.size() < 2 )
        return true;

    if( HUBO_PATH_RAW == params.interp )
    {
        return true;
    }
    
    std::vector<size_t> joint_mapping;
    get_active_indices(joint_mapping);
    
    std::vector<hubo_joint_limits_t> limits;
    if(!get_active_joint_limits(limits))
    {
        std::cout << "Cannot interpolate without knowing joint limits!" << std::endl;
        return false;
    }
    
    Eigen::VectorXd velocities(joint_mapping.size());
    Eigen::VectorXd accelerations(joint_mapping.size());
    for(size_t j=0; j<joint_mapping.size(); ++j)
    {
        velocities[j] = limits[j].nominal_speed;
        accelerations[j] = limits[j].nominal_accel;
    }
    
    double frequency = desc.okay()? desc.params.frequency : params.frequency;
    if( 0 == frequency )
    {
        std::cout << "Warning: Your Trajectory contained a frequency parameter of 0\n"
                     " -- We will default this to 200" << std::endl;
        frequency = 200;
    }
    
    if( HUBO_PATH_OPTIMAL == params.interp )
    {
        return _optimal_interpolation(velocities, accelerations, frequency);
    }
    else if( HUBO_PATH_SPLINE == params.interp )
    {
        return _spline_interpolation(velocities, accelerations, frequency);
    }
    else if( HUBO_PATH_DENSIFY == params.interp )
    {
        return _densify();
    }

    return false;
}
Ejemplo n.º 8
0
// --------------------------------------------------------------------------
//!
// --------------------------------------------------------------------------
void tCurrentInfoDialog::UpdateDialog()
{
    // Get the date time from the edit
    QDateTime dateTime( m_ActiveDate );

    // We want to update the graph here...
    QString errorMsg;
    if(m_pCurrentInfoObj->Query(dateTime, errorMsg, 10))
    {
        setWindowTitle( tr("Currents") + " - " + m_pCurrentInfoObj->Name() + " " + m_pCurrentInfoObj->m_stationNameAppend );

        // convert the heights data table into the correct format
        std::vector< tAt5CurrentsQuickInfoObject::tAt5CurrentsVelocity > At5Velocities = m_pCurrentInfoObj->m_currentsVelocities;
        QVector< QPair< qreal, tCurrentGraph::tCurrentVelocity > > velocities( At5Velocities.size() );
        for( uint i = 0; i < At5Velocities.size(); ++i )
        {
            QPair< qreal, tCurrentGraph::tCurrentVelocity > velocity;
            velocity.first = (qreal)i / 144;
            velocity.second.speed = (qreal)At5Velocities.at(i).speed;
            velocity.second.direction = (qreal)At5Velocities.at(i).direction;
            velocity.second.ebb = At5Velocities.at(i).ebb;
            velocities[i] = velocity;
        }
        m_pGraph->SetVelocities( velocities );

        // convert the extremes data table into the correct format
        std::vector< tAt5CurrentsQuickInfoObject::tAt5CurrentsExtreme > At5Extremes = m_pCurrentInfoObj->m_currentsExtremes;
        QVector< tCurrentGraph::tCurrentExtreme > extremes( At5Extremes.size() );
        for( uint i = 0; i < At5Extremes.size(); ++i )
        {
            tCurrentGraph::tCurrentExtreme extreme;
            extreme.time = At5Extremes.at(i).time.time();
            extreme.velocity.speed = At5Extremes.at(i).velocity.speed;
            extreme.velocity.direction = At5Extremes.at(i).velocity.direction;
            extreme.velocity.ebb = At5Extremes.at(i).velocity.ebb;
            extreme.type = (tCurrentGraph::tCurrentType)(At5Extremes.at(i).extreme_type);
            extremes[i] = extreme;
        }
        m_pGraph->SetExtremes( extremes );


        // sun rise, set
        int localOffset = tSystemSettings::Instance()->LocalTimeOffset() * 60;
        tSunData sunData;
        tSunMoon sunMoon;    
        sunData = sunMoon.SunCalculation( m_pCurrentInfoObj->GeoPosition().ToMCoord(), dateTime, localOffset );
        m_pGraph->SetSunRiseSet( sunData.Rise.time(), sunData.Set.time() );
    }
    m_pGraph->UpdateGraph(dateTime);
}
avtVector
avtIVPVTKOffsetField::operator()( const double &t, const avtVector &p ) const
{
    avtVector zeros(0, 0, 0);
    std::vector<avtVector> velocities(3);
    for ( size_t j = 0; j < 3; ++j )
    {

        if (FindCell(t, p) != OK)
        {
            // ghost cells on the base mesh may be required to avoid this failure
            debug5 <<"avtIVPVTKOffsetField::operator() - UNABLE TO FIND CELL!" 
                   <<std::endl;
            return zeros;
        }

        avtVector displ = GetPositionCorrection( j );
        avtVector pCorrected = p - displ;


        avtVector displ2 = displ;
        if (FindCell(t, pCorrected) != OK)
        {
            // the displacement seen from the base target position may be 
            // a little different.
            displ2 = GetPositionCorrection( j );
        }

        pCorrected = p - 0.5*(displ2 + displ);
        if (FindCell(t, pCorrected) != OK)
        {
            debug5 <<"avtIVPVTKOffsetField::operator() - UNABLE TO FIND CORRECTED CELL!" 
                   <<std::endl;
            return zeros;
        }        

        // velocity for this staggering
        FindValue(velData, velocities[j]);
    }

    // compose the velocity, assuming each component is purely 
    // aligned to each axis. How should this be generalized when
    // the velocites are not alog axes? 
    avtVector vel( velocities[0].x, velocities[1].y, velocities[2].z );
    return vel;
}
Ejemplo n.º 10
0
int fmm_stokes_solver(int ,char **)
{
    srand(0);
    typedef double value_type;
    size_t num_sources = 100;
    size_t num_targets = 100;
    size_t size_sources = 3*num_sources;
    size_t size_targets = 3*num_targets;
    std::vector<value_type> sources(size_sources), targets(size_targets), velocities(size_targets), forces(size_sources);
    value_type delta = .01;
    FmmStokesSolver<double,25,6> solver(num_sources);
    solver.setDelta(delta);

    std::generate(sources.begin(),sources.end(),random_generator<value_type>());
    std::generate(targets.begin(),targets.end(),random_generator<value_type>());
    std::generate(velocities.begin(),velocities.end(),random_generator<value_type>());
    std::generate(forces.begin(),forces.end(),random_generator<value_type>());

    solver(0,&targets[0],&velocities[0],&sources[0],&forces[0]);
    
    return 0;
}
Ejemplo n.º 11
0
void NormalDriver::set_predicted_vertex_positions( const SurfTrack& surf, std::vector<Vec3d>& predicted_positions, double current_t, double& adaptive_dt )
{
    std::vector<Vec3d> displacements( surf.get_num_vertices(), Vec3d(0,0,0) );
    std::vector<Vec3d> velocities( surf.get_num_vertices(), Vec3d(0,0,0) );
    
    for ( unsigned int i = 0; i < surf.get_num_vertices(); ++i )
    {
        if ( surf.m_mesh.m_vertex_to_triangle_map[i].empty() ) 
        { 
            displacements[i] = Vec3d(0,0,0);
            continue;
        }
        
        Vec3d normal(0,0,0);
        double sum_areas = 0.0;
        for ( unsigned int j = 0; j < surf.m_mesh.m_vertex_to_triangle_map[i].size(); ++j )
        {
            double area = surf.get_triangle_area( surf.m_mesh.m_vertex_to_triangle_map[i][j] );
            normal += surf.get_triangle_normal( surf.m_mesh.m_vertex_to_triangle_map[i][j] ) * area;
            sum_areas += area;
        }
        //normal /= sum_areas;
        normal /= mag(normal);
        
        double switch_speed = (current_t >= 1.0) ? -speed : speed;
        velocities[i] = switch_speed * normal;
        displacements[i] = adaptive_dt * velocities[i];
    }
    
    double capped_dt = MeshSmoother::compute_max_timestep_quadratic_solve( surf.m_mesh.get_triangles(), surf.get_positions(), displacements, false );
    
    adaptive_dt = min( adaptive_dt, capped_dt );
    
    for ( unsigned int i = 0; i < surf.get_num_vertices(); ++i )
    {
        predicted_positions[i] = surf.get_position(i) + adaptive_dt * velocities[i];
    }
    
}
void
MechanicContext::initVelocities(const /*Init*/Task& task)
{
  init(task);
  velocities(task);
}
bool MotomanJointTrajectoryStreamer::create_message_ex(int seq, const motoman_msgs::DynamicJointPoint &point, SimpleMessage *msg)
{
  JointTrajPtFullEx msg_data_ex;
  JointTrajPtFullExMessage jtpf_msg_ex;
  std::vector<industrial::joint_traj_pt_full::JointTrajPtFull> msg_data_vector;

  JointData values;

  int num_groups = point.num_groups;

  for (int i = 0; i < num_groups; i++)
  {
    JointTrajPtFull msg_data;

    motoman_msgs::DynamicJointsGroup pt;

    motoman_msgs::DynamicJointPoint dpoint;

    pt = point.groups[i];

    if (pt.positions.size() < 10)
    {
      int size_to_complete = 10 - pt.positions.size();

      std::vector<double> positions(size_to_complete, 0.0);
      std::vector<double> velocities(size_to_complete, 0.0);
      std::vector<double> accelerations(size_to_complete, 0.0);

      pt.positions.insert(pt.positions.end(), positions.begin(), positions.end());
      pt.velocities.insert(pt.velocities.end(), velocities.begin(), velocities.end());
      pt.accelerations.insert(pt.accelerations.end(), accelerations.begin(), accelerations.end());
    }

    // copy position data
    if (!pt.positions.empty())
    {
      if (VectorToJointData(pt.positions, values))
        msg_data.setPositions(values);
      else
        ROS_ERROR_RETURN(false, "Failed to copy position data to JointTrajPtFullMessage");
    }
    else
      msg_data.clearPositions();
    // copy velocity data
    if (!pt.velocities.empty())
    {
      if (VectorToJointData(pt.velocities, values))
        msg_data.setVelocities(values);
      else
        ROS_ERROR_RETURN(false, "Failed to copy velocity data to JointTrajPtFullMessage");
    }
    else
      msg_data.clearVelocities();

    // copy acceleration data
    if (!pt.accelerations.empty())
    {
      if (VectorToJointData(pt.accelerations, values))
        msg_data.setAccelerations(values);
      else
        ROS_ERROR_RETURN(false, "Failed to copy acceleration data to JointTrajPtFullMessage");
    }
    else
      msg_data.clearAccelerations();

    // copy scalar data
    msg_data.setRobotID(pt.group_number);
    msg_data.setSequence(seq);
    msg_data.setTime(pt.time_from_start.toSec());

    // convert to message
    msg_data_vector.push_back(msg_data);
  }

  msg_data_ex.setMultiJointTrajPtData(msg_data_vector);
  msg_data_ex.setNumGroups(num_groups);
  msg_data_ex.setSequence(seq);
  jtpf_msg_ex.init(msg_data_ex);

  return jtpf_msg_ex.toRequest(*msg);  // assume "request" COMM_TYPE for now
}
particle_swarm_optimisation<T, N>::particle_swarm_optimisation() noexcept
    : optimiser<T, N>(),
      initial_velocity(T(0.5)),
      maximal_acceleration(T(1.0) / (T(2.0) * std::log(T(2.0)))),
      maximal_local_attraction(T(0.5) + std::log(T(2.0))),
      maximal_global_attraction(maximal_local_attraction) {
  this->optimisation_function = [this](const mant::problem<T, N>& problem, std::vector<std::array<T, N>> parameters) {
    assert(initial_velocity >= T(0.0));
    assert(maximal_acceleration >= T(0.0));
    assert(maximal_local_attraction >= T(0.0));
    assert(maximal_global_attraction >= T(0.0));

    auto&& start_time  = std::chrono::steady_clock::now();
    optimise_result<T, N> result;

    std::vector<std::array<T, N>> velocities(parameters.size());
    for (auto& velocity : velocities) {
      std::generate(
        velocity.begin(), std::next(velocity.begin(), this->active_dimensions.size()),
        std::bind(std::uniform_real_distribution<T>(-initial_velocity, initial_velocity), std::ref(random_number_generator()))
      );
    }

    std::vector<std::array<T, N>> local_parameters = parameters;
    std::vector<T> local_objective_values(local_parameters.size());

    for (std::size_t n = 0; n < parameters.size(); ++n) {
      const auto& parameter = parameters.at(n);
      const auto objective_value = problem.objective_function(parameter);
      ++result.evaluations;
      result.duration = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now() - start_time);

      local_objective_values.at(n) = objective_value;
      
      if (objective_value <= result.objective_value) {
        result.parameter = parameter;
        result.objective_value = objective_value;

        if (result.objective_value <= this->acceptable_objective_value) {
          return result;
        }
      }

      if (result.evaluations >= this->maximal_evaluations) {
        return result;
      } else if (result.duration >= this->maximal_duration) {
        return result;
      }
    }

    while (result.duration < this->maximal_duration && result.evaluations < this->maximal_evaluations && result.objective_value > this->acceptable_objective_value) {
      const auto n = result.evaluations % parameters.size();
      auto& parameter = parameters.at(n);
      const auto& local_parameter = local_parameters.at(n);

      std::array<T, N> attraction_center;
      const auto weigthed_local_attraction = maximal_local_attraction * std::uniform_real_distribution<T>(0, 1)(random_number_generator());
      const auto weigthed_global_attraction = maximal_global_attraction * std::uniform_real_distribution<T>(0, 1)(random_number_generator());
      for (std::size_t k = 0; k < this->active_dimensions.size(); ++k) {
        attraction_center.at(k) = (
            weigthed_local_attraction * (local_parameter.at(k) - parameter.at(k)) +
            weigthed_global_attraction * (result.parameter.at(k) - parameter.at(k))) /
          T(3.0);
      }

      const auto&& random_velocity = random_neighbour(
        attraction_center,
        T(0.0),
        std::sqrt(std::inner_product(
          attraction_center.cbegin(), attraction_center.cend(),
          attraction_center.cbegin(),
          T(0.0))),
        this->active_dimensions.size());

      auto& velocity = velocities.at(n);
      const auto weigthed_acceleration = maximal_acceleration * std::uniform_real_distribution<T>(0, 1)(random_number_generator());
      for (std::size_t k = 0; k < this->active_dimensions.size(); ++k) {
        auto& parameter_element = parameter.at(k);
        auto& velocity_element = velocity.at(k);

        velocity_element = weigthed_acceleration * velocity_element + random_velocity.at(k);
        parameter_element += velocity_element;

        if (parameter_element < T(0.0)) {
          parameter_element = T(0.0);
          velocity_element *= -T(0.5);
        } else if(parameter_element > T(1.0)) {
          parameter_element = T(1.0);
          velocity_element *= -T(0.5);
        }
      }

      const auto objective_value = problem.objective_function(parameter);
      ++result.evaluations;
      result.duration = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::steady_clock::now() - start_time);

      if (objective_value < result.objective_value) {
        result.parameter = parameter;
        result.objective_value = objective_value;
        local_parameters.at(n) = parameter;
        local_objective_values.at(n) = objective_value;
      } else if (objective_value < local_objective_values.at(n)) {
        local_parameters.at(n) = parameter;
        local_objective_values.at(n) = objective_value;
      }
    }

    return result;
  };
}
Ejemplo n.º 15
0
void simulationLoop(std::vector<double>& vertices, std::vector<Edge>& edges, double dt){


  std::vector<double> velocities(vertices.size(), 0);


  std::chrono::time_point<std::chrono::high_resolution_clock> start, end, renderEnd;
  //size_t start, end, renderEnd;
  renderEnd = std::chrono::high_resolution_clock::now();
  while(true){
  
    SDL_Event event;
    
    while(SDL_PollEvent(&event)){

      switch(event.type){
      case SDL_KEYDOWN:
	exit(0);
      default:
	0;// do nothing
      }
    }

    start = std::chrono::high_resolution_clock::now();
    
    simulateFrame(vertices, velocities, edges, dt);
    end = std::chrono::high_resolution_clock::now();
    
    //std::chrono::microseconds elapsed_u_seconds = end- renderEnd;//start;
    
    auto microseconds_elapsed = std::chrono::duration_cast<std::chrono::microseconds>(end - renderEnd);
    auto dtInMicroseconds = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::duration<double>(dt));

    //std::cout << "elapsed: " << microseconds_elapsed.count() << "  dt: "  << dtInMicroseconds.count() << std::endl;;

    /*if(elapsed_seconds.count() < dt){
      std::this_thread::sleep_for(std::chrono::duration<double>(dt - elapsed_seconds.count()
								));
      std::cout << "sim took: " << 1.0/elapsed_seconds.count() << " hz" << std::endl;
      std::cout << "slept for : " << std::chrono::duration<double>(dt - elapsed_seconds.count()).count() << std::endl;
      } */
    //while( (std::chrono::high_resolution_clock::now() - renderEnd).count() < dt){
      //spin
    //}
    //std::cout << "waited " << (std::chrono::high_resolution_clock::now() - end).count() << " seconds" << std::endl;

    /*if((end - renderEnd) < (1000*dt)){
      auto toWait = (1000*dt) - (end - renderEnd);
      std::cout << "waiting " << toWait << "ms" << std::endl;
      SDL_Delay(toWait);

      }*/

    while(std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - renderEnd).count() <
	  dtInMicroseconds.count()){
      //std::this_thread::sleep_for(dtInMicroseconds - microseconds_elapsed);
    }
    //std::cout << "simulation took " << microseconds_elapsed.count() << "us" << std::endl;
    //std::cout << "waited: " << std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - end).count() << "us" << std::endl;
    drawFrame(vertices, edges);


    renderEnd = std::chrono::high_resolution_clock::now();

    
    
  }
}
vector Foam::cfdemCloud::velocity(int index)
{
    vector vel;
    for(int i=0;i<3;i++) vel[i] = velocities()[index][i];
    return vel;
}
Ejemplo n.º 17
0
void Engine::update(float dt) {

    // look //
    double x, y;
    glfwGetCursorPos(window, &x, &y);
    static double last_x = x, last_y = y;
    static float x_rot = glm::asin(camera.get_forward().y),
                 y_rot = glm::asin(camera.get_forward().x);

    double dx = last_x - x, dy = last_y - y;

    if (glfwGetKey(window, GLFW_KEY_K) == GLFW_PRESS) {
        x_rot += dt * fake_mouse_change;
    }
    if (glfwGetKey(window, GLFW_KEY_J) == GLFW_PRESS) {
        x_rot -= dt * fake_mouse_change;
    }
    if (glfwGetKey(window, GLFW_KEY_H) == GLFW_PRESS) {
        y_rot -= dt * fake_mouse_change;
    }
    if (glfwGetKey(window, GLFW_KEY_L) == GLFW_PRESS) {
        y_rot += dt * fake_mouse_change;
    }

    last_x = x;
    last_y = y;

    // positive y_rot means left.
    y_rot -= mouse_sensitivity * dx;
    x_rot += mouse_sensitivity * dy;

    float max_angle = glm::radians(85.f);
    float min_angle = -max_angle;

    if (x_rot > max_angle) {
        x_rot = max_angle;
    } else if (x_rot < min_angle) {
        x_rot = min_angle;
    }

    glm::vec3 forward = glm::vec3(
            cos(x_rot) * sin(y_rot),
            sin(x_rot),
            -cos(x_rot) * cos(y_rot));
    camera.lookat = camera.position + forward;


    // movement //
    glm::vec3 move_dir(0.f),
              right = camera.get_right(),
              up = glm::normalize(camera.up);

    if (glfwGetKey(window, GLFW_KEY_W) == GLFW_PRESS) {
        move_dir += forward;
    }
    if (glfwGetKey(window, GLFW_KEY_S) == GLFW_PRESS) {
        move_dir -= forward;
    }
    if (glfwGetKey(window, GLFW_KEY_D) == GLFW_PRESS) {
        move_dir += right;
    }
    if (glfwGetKey(window, GLFW_KEY_A) == GLFW_PRESS) {
        move_dir -= right;
    }
    if (glfwGetKey(window, GLFW_KEY_E) == GLFW_PRESS) {
        move_dir += up;
    }
    if (glfwGetKey(window, GLFW_KEY_Q) == GLFW_PRESS) {
        move_dir -= up;
    }

    float move_speed = normal_move_speed;
    if (glfwGetKey(window, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS) {
        move_speed /= move_speed_change_factor;
    }
    if (glfwGetKey(window, GLFW_KEY_LEFT_CONTROL) == GLFW_PRESS) {
        move_speed *= move_speed_change_factor;
    }

    float len = glm::length(move_dir);
    if (len > 1.f) {
        move_dir = move_dir / len;
    }

    if (glfwGetKey(window, GLFW_KEY_SPACE) == GLFW_PRESS) {
        dt *= 10;
    }
    if (glfwGetKey(window, GLFW_KEY_V) == GLFW_PRESS) {
        dt /= 10;
    }

    // Note: assumes dt will be >= 0. No timetravel...
    glm::vec3 move = dt * move_speed * move_dir;
    camera.position += move;
    camera.lookat += move;

    // physics //
    bool paused = (glfwGetKey(window, GLFW_KEY_C) == GLFW_RELEASE);
    if (!paused) {
        physics->step(dt);

        //auto printbtvec = [] (const char* fmt, btVector3 v) { // TODO
        //    printf(fmt, v.x(), v.y(), v.z());
        //};

        btScalar pi = SIMD_PI;

        btVector3 local(0,0.2f,0);
        btVector3 target = from_vec3(nodes["redcube"]->position);

        KinematicChain* arm = roboarm3;

        std::vector<btScalar> angles = arm->get_angles();
        std::vector<btScalar> tar_angles =
                arm->calc_angles_ccd(angles, target, local);

        btVector3 dest = arm->get_position(tar_angles, local);

        glm::vec3 offset = glm::vec3(0.05f, 0.f, 0.f);
        nodes["cyancube"]->position = to_vec3(dest) - offset;
        nodes["yellowcube"]->position = to_vec3(dest) + offset;

        std::vector<btScalar> velocities(angles.size());

        for (size_t i = 0; i < angles.size(); ++i) {
            velocities[i] = (i+1)*7000.f*(tar_angles[i] - angles[i])*dt;
        }

        arm->apply_velocities(velocities);

        //for (size_t i = 0; i < angles.size(); ++i) { // TODO
        //    printf("v[%lu] %f-%f ==> %f\n", i, angles[i], tar_angles[i],
        //                               velocities[i]);
        //}
    }

    if (glfwGetKey(window, GLFW_KEY_KP_ADD) == GLFW_PRESS) {
        nodes["redcube"]->position +=
                move_speed * dt * glm::vec3(0.f, 1.f, 0.f);
    }
    if (glfwGetKey(window, GLFW_KEY_KP_SUBTRACT) == GLFW_PRESS) {
        nodes["redcube"]->position +=
                move_speed * dt * glm::vec3(0.f, -1.f, 0.f);
    }
    if (glfwGetKey(window, GLFW_KEY_KP_2) == GLFW_PRESS) {
        nodes["redcube"]->position +=
                move_speed * dt * glm::vec3(0.f, 0.f, 1.f);
    }
    if (glfwGetKey(window, GLFW_KEY_KP_8) == GLFW_PRESS) {
        nodes["redcube"]->position +=
                move_speed * dt * glm::vec3(0.f, 0.f, -1.f);
    }

    if (glfwGetKey(window, GLFW_KEY_KP_4) == GLFW_PRESS) {
        nodes["redcube"]->position +=
                move_speed * dt * glm::vec3(-1.f, 0.f, 0.f);
    }
    if (glfwGetKey(window, GLFW_KEY_KP_6) == GLFW_PRESS) {
        nodes["redcube"]->position +=
                move_speed * dt * glm::vec3(1.f, 0.f, 0.f);
    }

    redcube_rb->setMotionState(redcube_rb->getMotionState());

    nodes["cyancube"]->orientation *=
            glm::angleAxis(dt*glm::radians(90.f),
                           glm::normalize(glm::vec3(1.f, 1.f, 1.f)));

    // quit //
    if (glfwGetKey(window, GLFW_KEY_ESCAPE) == GLFW_PRESS) {
        glfwSetWindowShouldClose(window, GL_TRUE);
    }
}