Beispiel #1
0
/*
 * 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);
	}
Beispiel #5
0
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;
}
Beispiel #6
0
        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());
}
Beispiel #11
0
double ASystem::frameRate()
{
	return 1000.0/getTimeStep ();
}
Beispiel #12
0
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));
	}