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