/*
* Reads keyboard input and updates scene variables.
* 'w' moves forward, 's' moves back
* 'a' strafes left, 'd' strafes right
* 'r' resets the view
* 'f' Sets movement to free, 'p' sets movement to person
*/
void input_update () {
float distance = 60.0f * getTimeStep();
static float walkTheta = 0;
vect3_t old_position, boundingBox = {1,1,1};
VectorCopy(camera.position, old_position);
// walk motion
if (keys_down[SDLK_w] || keys_down[SDLK_s]) {
walkTheta -= getTimeStep();
walkHeight = WALK_POSITION_SCALE*sin(walkTheta * WALK_TIME_SCALE);
}
// WASD movement
if(keys_down[SDLK_w]) {
camera_translateForward(-1 * distance);
}
if(keys_down[SDLK_s]) {
camera_translateForward(distance);
}
if(keys_down[SDLK_a])
camera_translateStrafe(-1 * distance);
if(keys_down[SDLK_d])
camera_translateStrafe(distance);
if (world_testCollision(boundingBox))
VectorCopy(old_position, camera.position);
if(keys_down[SDLK_z])
camera.position[_X] += 1;
if(keys_down[SDLK_x])
camera.position[_X] -= 1;
if(keys_down[SDLK_c])
camera.position[_Y] += 1;
if(keys_down[SDLK_v])
camera.position[_Y] -= 1;
if(keys_down[SDLK_b])
camera.position[_Z] += 1;
if(keys_down[SDLK_n])
camera.position[_Z] -= 1;
// r for reset
if(keys_down[SDLK_r]){
reset_position();
}
// Set movement to free or person
if (keys_down[SDLK_f])
free_move = 1;
else if (keys_down[SDLK_p])
free_move = 0;
}
void
BodySensor::sensorModel(){
double forces[6] = {0,0,0,0,0,0};
std::list<Contact *>::const_iterator cp;
std::list<Contact *> cList = sbody->getContacts();
//Adding contacts
for(cp = cList.begin(); cp != cList.end(); cp++){
double * contactForce;
if(sbody->getWorld()->dynamicsAreOn())
contactForce = (*cp)->getDynamicContactWrench();
else{
double tempForce[6] = {0, 0, 1, 0, 0, 0};
contactForce = tempForce;
}
addVector6(forces, contactForce);
}
//std::cout << "Body Sensor Reading: ";
//Adding Forces
double ts = getTimeStep();
if (ts > 0.0)
for(int ind = 0; ind < 6; ind++){
myOutput.sensorReading[ind] = forces[ind] * (retention_level) + myOutput.sensorReading[ind] * (1.0-retention_level);
// std::cout << myOutput.sensorReading[ind] << " ";
}
//std::cout<<std::endl;
sbody->setEmColor(myOutput.sensorReading[2]/3.0,0,1);
}
void
RegionFilteredSensor::sensorModel(){
double forces[6] = {0,0,0,0,0,0};
std::list<Contact *>::const_iterator cp;
std::list<Contact *> cList = sbody->getContacts();
//Adding contacts
if(sbody->getWorld()->dynamicsAreOn()){
for(cp = cList.begin(); cp != cList.end(); cp++){
double * contactForce = (*cp)->getDynamicContactWrench();
//std::cout << "Contact pos:" << (*cp)->getPosition()[0] <<" " <<(*cp)->getPosition()[1] <<" "<< (*cp)->getPosition()[2] << std::endl;
if (filterContact(*cp))
addVector6(forces, contactForce);
}
//Adding Forces
double ts = getTimeStep();
if (ts > 0.0)
for(int ind = 0; ind < 6; ind++){
myOutput.sensorReading[ind] = forces[ind] * (retention_level) + myOutput.sensorReading[ind] * (1.0-retention_level);
//std::cout << myOutput.sensorReading[ind] << " ";
}
}
else{
myOutput.sensorReading[2] = 0;
for(cp = cList.begin(); cp != cList.end(); cp++){
if(sbody->getWorld()->softContactsAreOn() && ((*cp)->getBody1()->isElastic() || (*cp)->getBody2()->isElastic())){
std::vector<position> pVec;
std::vector<double> forceVec;
(*cp)->getStaticContactInfo(pVec, forceVec);
//transform the boundary positions OUT of body coordinates to contact coordinates
//this results in fewer expensive matrix operations
//fixme test that this is correct
transf contact = (*cp)->getContactFrame();//.inverse();
/*For the sake of argument, lets say we take the position of the contact on the body,
and look at how that works
*/
mat3 contactRot = (*cp)->getRot();
for (unsigned int pInd = 0; pInd < pVec.size(); pInd ++){
//it appears that the contact frame is a roto-inversion
if(filterContact(pos[0],pos[1], (contactRot*(1000*pVec[pInd]))*contact))
myOutput.sensorReading[2]+= forceVec[pInd]* 10000000;
}
}
else if (filterContact(*cp)){
myOutput.sensorReading[2] += 1;
}
}
}
IVMat->emissiveColor.setValue(1.0-myOutput.sensorReading[2]/3,1.0,1.0-myOutput.sensorReading[2]/3);
//std::cout<<std::endl;
}
void setupInitialConditions()
{
relVelocity_ = 1e-1;
setName("AdhesiveForceUnitTest_ParticleWallInteractionWithPlasticForces");
setSystemDimensions(3);
setParticleDimensions(3);
setGravity(Vec3D(0,0,0));
species->setDensity(2000.0);
species->setStiffness(1e2);
species->setAdhesionStiffness(1e2);
species->setAdhesionForceMax(1e-5*species->getAdhesionStiffness());
Mdouble R = 1e-3;
species->setSlidingStiffness(1.2e1);
species->setSlidingFrictionCoefficient(0.01);
setTimeStep(5e-6 / 2.0);
setXMax( 2*R);
setYMax( R);
setZMax( R);
setXMin(0);
setYMin(-R);
setZMin(-R);
particleHandler.clear();
BaseParticle P;
P.setRadius(R);
P.setPosition(Vec3D(R+species->getInteractionDistance()*1/3,0,0));
P.setVelocity(Vec3D(-relVelocity_/2,0,0));
particleHandler.copyAndAddObject(P);
wallHandler.clear();
InfiniteWall w;
w.set(Vec3D(-1, 0, 0), Vec3D(getXMin(), 0, 0));
wallHandler.copyAndAddObject(w);
setTimeMax(getTimeStep()*250*4);
setFileType(FileType::ONE_FILE);
setSaveCount(1);
}
void NetCdfConfigureDialog::createDataObject()
{
size_t* length = new size_t[_currentVar->num_dims()];
double originLon = 0, originLat = 0;
double lastLon = 0, lastLat = 0;
unsigned sizeLon = 0, sizeLat = 0;
getDimEdges(comboBoxDim1->currentIndex(), sizeLat, originLat, lastLat);
getDimEdges(comboBoxDim2->currentIndex(), sizeLon, originLon, lastLon);
for(int i=0; i < _currentVar->num_dims(); i++) length[i]=1;
// set array edges: lat x lon
length[comboBoxDim1->currentIndex()]=sizeLat;
length[comboBoxDim2->currentIndex()]=sizeLon;
// set up array
double* data_array = new double[sizeLat*sizeLon];
for(size_t i=0; i < (sizeLat*sizeLon); i++) data_array[i]=0;
//Time-Dimension:
if (_currentVar->num_dims() > 2)
{
long* newOrigin = new long[_currentVar->num_dims()];
for (int i=0; i < _currentVar->num_dims(); i++) newOrigin[i]=0;
newOrigin[comboBoxDim3->currentIndex()] = getTimeStep(); //set origin to selected time
_currentVar->set_cur(newOrigin);
//Dimension 4:
if (_currentVar->num_dims() > 3) newOrigin[comboBoxDim4->currentIndex()] = getDim4(); //if there are is a 4th dimension
delete newOrigin;
}
_currentVar->get(data_array,length); //create Array of Values
for (size_t i=0; i < (sizeLat*sizeLon); i++)
{
//data_array[i] = data_array[i] - 273; // convert from kalvin to celsius
if (data_array[i] < -9999 ) data_array[i] = -9999; // all values < -10000, set to "no-value"
}
double origin_x = (originLon < lastLon) ? originLon : lastLon;
double origin_y = (originLat < lastLat) ? originLat : lastLat;
double originNetCdf[3] = {origin_x, origin_y, 0};
double resolution = (doubleSpinBoxResolution->value());
if (originLat > lastLat) // reverse lines in vertical direction if the original file has its origin in the northwest corner
this->reverseNorthSouth(data_array, sizeLon, sizeLat);
if (this->radioMesh->isChecked())
{
MeshElemType meshElemType = MeshElemType::QUAD;
UseIntensityAs useIntensity = UseIntensityAs::MATERIAL;
if ((comboBoxMeshElemType->currentIndex()) == 1)
{
meshElemType = MeshElemType::TRIANGLE;
} else {
meshElemType = MeshElemType::QUAD;
}
if ((comboBoxUseIntensity->currentIndex()) == 1)
{
useIntensity = UseIntensityAs::ELEVATION;
} else {
useIntensity = UseIntensityAs::MATERIAL;
}
_currentMesh = VtkMeshConverter::convertImgToMesh(data_array,originNetCdf,sizeLon,sizeLat,resolution,meshElemType,useIntensity);
}
else
{
vtkImageImport* image = VtkRaster::loadImageFromArray(data_array, originNetCdf[0], originNetCdf[1], sizeLon, sizeLat, resolution, -9999.0);
_currentRaster = VtkGeoImageSource::New();
_currentRaster->setImage(image, QString::fromStdString(this->getName()), originNetCdf[0], originNetCdf[1], resolution);
}
delete[] length;
delete[] data_array;
}
void
task(void)
{
if(!BasicNavigation::isActive())
return;
// Compute time delta.
double tstep = getTimeStep();
// Check if we have a valid time delta.
if (tstep < 0)
return;
m_heading += Angles::minSignedAngle(m_heading, Angles::normalizeRadian(getEuler(AXIS_Z)));
m_estate.psi = Angles::normalizeRadian(m_heading);
m_estate.r = getAngularVelocity(AXIS_Z);
// Update estimated state.
onDispatchNavigation();
// Some (experimental) sanity checks. This is not standard EKF!
// If any of this conditions is met, some kind of warning should be raised.
double wx = Math::trimValue(m_kal.getState(STATE_WX), -m_args.max_current, m_args.max_current);
double wy = Math::trimValue(m_kal.getState(STATE_WY), -m_args.max_current, m_args.max_current);
m_kal.setState(STATE_WX, wx);
m_kal.setState(STATE_WY, wy);
if (m_kal.getCovariance(STATE_WX) > std::pow(m_args.max_current, 2))
m_kal.setProcessNoise(STATE_WX, 0);
else
m_kal.setProcessNoise(STATE_WX, m_process_noise[1] * tstep);
if (m_kal.getCovariance(STATE_WY) > std::pow(m_args.max_current, 2))
m_kal.setProcessNoise(STATE_WY, 0);
else
m_kal.setProcessNoise(STATE_WY, m_process_noise[1] * tstep);
// Reset and discretize transition matrix function.
Matrix a(NUM_STATE, NUM_STATE, 0.0);
a(STATE_X, STATE_K) = m_rpm * std::cos(m_estate.theta) * std::cos(m_estate.psi);
a(STATE_Y, STATE_K) = m_rpm * std::cos(m_estate.theta) * std::sin(m_estate.psi);
a(STATE_X, STATE_WX) = 1.0;
a(STATE_Y, STATE_WY) = 1.0;
m_kal.setTransitions((a * tstep).expmts());
// Run EKF prediction.
m_kal.predict();
checkUncertainty();
// Fill and dispatch data.
m_estate.u = m_rpm * m_kal.getState(STATE_K) * std::cos(m_estate.theta);
// Consider water speed to get ground speed
m_estate.vx += m_kal.getState(STATE_WX);
m_estate.vy += m_kal.getState(STATE_WY);
// Log Navigation Uncertainty.
m_uncertainty.u = m_kal.getCovariance(STATE_K);
// Log Navigation Data.
m_navdata.custom_x = m_kal.getCovariance(STATE_WX);
m_navdata.custom_y = m_kal.getCovariance(STATE_WY);
m_navdata.custom_z = m_kal.getState(STATE_K);
m_navdata.cog = std::atan2(m_estate.vy, m_estate.vx);
// Fill and dispatch estimated water velocity message.
m_ewvel.x = m_kal.getState(STATE_WX);
m_ewvel.y = m_kal.getState(STATE_WY);
m_ewvel.z = 0;
reportToBus();
updateBuffers(c_wma_filter);
}
void setupInitialConditions()
{
setParticleDimensions(3);
setXMax(2);
setYMax(2);
setZMax(2);
setSystemDimensions(3);
species->setDensity(6.0 / constants::pi);
setGravity(Vec3D(0.0, 0.0, 0.0));
species->setCollisionTimeAndRestitutionCoefficient(0.01, 1.0, 1.0);
setTimeStep(0.0002);
setTimeMax(1.0);
setSaveCount(helpers::getSaveCountFromNumberOfSavesAndTimeMaxAndTimestep(20, getTimeMax(), getTimeStep()));
//set particles
particle->setRadius(0.5);
particle->setPosition(Vec3D(1.0, 1.0, 1.0));
particle->setVelocity(Vec3D(0.0, 0.0, -1.0));
//set walls
wall->setNormal(Vec3D(0.0, 0.0, -1.0));
}
void TargetPredConfigWidget::onConfigsChanged() {
typedef ctrl::satellite_predictor_options sat_opt;
sat_options.system_kind = 0;
switch(this->kf_model_selection->currentIndex()) {
case 0: // IEKF
sat_options.system_kind |= sat_opt::invariant;
break;
case 1: // IMKF
sat_options.system_kind |= sat_opt::invar_mom;
break;
case 2: // IMKFv2
sat_options.system_kind |= sat_opt::invar_mom2;
break;
case 3: // IMKF_em
sat_options.system_kind |= sat_opt::invar_mom_em;
break;
case 4: // IMKF_emd
sat_options.system_kind |= sat_opt::invar_mom_emd;
break;
case 5: // IMKF_emdJ
sat_options.system_kind |= sat_opt::invar_mom_emdJ;
break;
case 6: // TSOSAIKF_em
sat_options.system_kind |= sat_opt::invar_mom_em | sat_opt::TSOSAKF;
break;
case 7: // TSOSAIKF_emd
sat_options.system_kind |= sat_opt::invar_mom_emd | sat_opt::TSOSAKF;
break;
case 8: // TSOSAIKF_emdJ
sat_options.system_kind |= sat_opt::invar_mom_emdJ | sat_opt::TSOSAKF;
break;
};
if(this->gyro_check->isChecked())
sat_options.system_kind |= sat_opt::gyro_measures;
if(this->IMU_check->isChecked())
sat_options.system_kind |= sat_opt::IMU_measures;
switch(this->predict_assumption_selection->currentIndex()) {
case 0: // No future measurements
sat_options.predict_assumption = sat_opt::no_measurements;
break;
case 1: // Maximum-likelihood Measurements
sat_options.predict_assumption = sat_opt::most_likely_measurements;
break;
case 2: // Full certainty
sat_options.predict_assumption = sat_opt::full_certainty;
break;
};
sat_options.time_step = getTimeStep();
sat_options.mass = getMass();
sat_options.inertia_tensor = getInertiaTensor();
sat_options.input_disturbance = getInputDisturbance();
sat_options.measurement_noise = getMeasurementNoise();
sat_options.IMU_orientation = getIMUOrientation();
sat_options.IMU_location = getIMULocation();
sat_options.earth_orientation = getEarthOrientation();
sat_options.mag_field_direction = getMagFieldDirection();
sat_options.initial_motion = frame_3D<double>();
sat_options.predict_time_horizon = getTimeHorizon();
sat_options.predict_Pnorm_threshold = getPThreshold();
};
void PlottingControlQueue::addJointsPosToQueue(arma::vec joints) {
currJoints = joints;
currTime += getTimeStep();
}
TEST(NumLibTimeStepping, testEvolutionaryPIDcontroller)
{
const char xml[] =
"<time_stepping>"
" <type>EvolutionaryPIDcontroller</type>"
" <t_initial> 0.0 </t_initial>"
" <t_end> 10 </t_end>"
" <dt_guess> 0.01 </dt_guess>"
" <dt_min> 0.001 </dt_min>"
" <dt_max> 1 </dt_max>"
" <rel_dt_min> 0.01 </rel_dt_min>"
" <rel_dt_max> 5 </rel_dt_max>"
" <tol> 1.e-3 </tol>"
"</time_stepping>";
auto const PIDStepper = createTestTimeStepper(xml);
double solution_error = 0.;
int const number_iterations = 0;
// 1st step
ASSERT_TRUE(PIDStepper->next(solution_error, number_iterations));
NumLib::TimeStep ts = PIDStepper->getTimeStep();
double h_new = 0.01;
double t_previous = 0.;
ASSERT_EQ(1u, ts.steps());
ASSERT_EQ(t_previous, ts.previous());
ASSERT_EQ(t_previous + h_new, ts.current());
ASSERT_EQ(h_new, ts.dt());
ASSERT_TRUE(PIDStepper->accepted());
t_previous += h_new;
// e_n_minus1 is filled.
solution_error = 1.0e-4;
PIDStepper->next(solution_error, number_iterations);
ts = PIDStepper->getTimeStep();
h_new = ts.dt();
ASSERT_EQ(2u, ts.steps());
const double tol = 1.e-16;
ASSERT_NEAR(t_previous, ts.previous(), tol);
ASSERT_NEAR(t_previous + h_new, ts.current(), tol);
ASSERT_TRUE(PIDStepper->accepted());
t_previous += h_new;
// e_n_minus2 is filled.
solution_error = 0.5e-3;
PIDStepper->next(solution_error, number_iterations);
ts = PIDStepper->getTimeStep();
h_new = ts.dt();
ASSERT_EQ(3u, ts.steps());
ASSERT_NEAR(t_previous, ts.previous(), tol);
ASSERT_NEAR(t_previous + h_new, ts.current(), tol);
ASSERT_TRUE(PIDStepper->accepted());
// error > TOL=1.3-3, step rejected and new step size estimated.
solution_error = 0.01;
PIDStepper->next(solution_error, number_iterations);
ts = PIDStepper->getTimeStep();
h_new = ts.dt();
// No change in ts.steps
ASSERT_EQ(3u, ts.steps());
// No change in ts.previous(), which is the same as that of the previous
// step.
ASSERT_NEAR(t_previous, ts.previous(), tol);
ASSERT_NEAR(t_previous + h_new, ts.current(), tol);
ASSERT_FALSE(PIDStepper->accepted());
t_previous += h_new;
// With e_n, e_n_minus1, e_n_minus2
solution_error = 0.4e-3;
PIDStepper->next(solution_error, number_iterations);
ts = PIDStepper->getTimeStep();
h_new = ts.dt();
ASSERT_EQ(4u, ts.steps());
ASSERT_NEAR(t_previous, ts.previous(), tol);
ASSERT_NEAR(t_previous + h_new, ts.current(), tol);
ASSERT_TRUE(PIDStepper->accepted());
}
double ASystem::frameRate()
{
return 1000.0/getTimeStep ();
}
int ASystem::timeStep()
{
return getTimeStep();
}
void setupInitialConditions()
{
setParticleDimensions(2);
setSystemDimensions(2);
setXMin(-2.0);
setXMax(2.0);
setYMin(-2.0);
setYMax(2.0);
species->setDensity(6.0 / constants::pi);
setGravity(Vec3D(0.0, 0.0, 0.0));
species->setCollisionTimeAndRestitutionCoefficient(0.01, 1.0, 1.0);
setTimeStep(0.0002);
setTimeMax(1.0);
setSaveCount(helpers::getSaveCountFromNumberOfSavesAndTimeMaxAndTimestep(20, getTimeMax(), getTimeStep()));
p1f = particleHandler.copyAndAddObject(BaseParticle());
p2f = particleHandler.copyAndAddObject(BaseParticle());
p3f = particleHandler.copyAndAddObject(BaseParticle());
p1e = particleHandler.copyAndAddObject(BaseParticle());
p2e = particleHandler.copyAndAddObject(BaseParticle());
p3e = particleHandler.copyAndAddObject(BaseParticle());
triangle = wallHandler.copyAndAddObject(IntersectionOfWalls());
std::vector<Vec3D> points={Vec3D(0.5,-std::sqrt(3)/6.0,0.0),Vec3D(0.0,std::sqrt(3)/3.0,0.0),Vec3D(-0.5,-std::sqrt(3)/6.0,0.0),Vec3D(0.5,-std::sqrt(3)/6.0,0.0)};
triangle->createOpenPrism(points,Vec3D(0.0,0.0,1.0));
p1f->setRadius(0.1);
p2f->setRadius(0.1);
p3f->setRadius(0.1);
p1f->setPosition(Vec3D(-0.5, std::sqrt(3)/6.0,0.0));
p2f->setPosition(Vec3D(-0.0,-std::sqrt(3)/3.0,0.0));
p3f->setPosition(Vec3D( 0.5, std::sqrt(3)/6.0,0.0));
p1f->setVelocity(Vec3D( 0.5,-std::sqrt(3)/6.0,0.0));
p2f->setVelocity(Vec3D( 0.0, std::sqrt(3)/3.0,0.0));
p3f->setVelocity(Vec3D(-0.5,-std::sqrt(3)/6.0,0.0));
p1e->setRadius(0.1);
p2e->setRadius(0.1);
p3e->setRadius(0.1);
p1e->setPosition(1.5*Vec3D( 0.5,-std::sqrt(3)/6.0,0.0));
p2e->setPosition(1.5*Vec3D( 0.0, std::sqrt(3)/3.0,0.0));
p3e->setPosition(1.5*Vec3D(-0.5,-std::sqrt(3)/6.0,0.0));
p1e->setVelocity(Vec3D(-0.5, std::sqrt(3)/6.0,0.0));
p2e->setVelocity(Vec3D( 0.0,-std::sqrt(3)/3.0,0.0));
p3e->setVelocity(Vec3D( 0.5, std::sqrt(3)/6.0,0.0));
}
