//------------------------------------------------------------------------------
void AdaptiveDynamicRelaxation::initialConfig()
{
// Zeroing the intial force vectors
// and setting the stable mass matrix
#ifdef _OPENMP
# pragma omp parallel for schedule(runtime)
#endif
for(int i=0;i<domain->myGrid.size(); i++)
{
int gridId = domain->myGrid[i];
GridPoint * gridPoint = domain->grid[gridId];
for(Particle * p1:gridPoint->getParticles())
{
VEC3 & f = p1->f();
int pId = p1->id();
for(int d=0; d<dim; d++){
f[d] = 0;
Fold[pId*dim + d] = 0;
vOldhalf[pId*dim + d] = 0;
vHalf[pId*dim + d] = 0;
stableMass[pId*dim + d] = pdForces[0]->stableMass(p1, dt);
}
}
}
createVerletList();
calculateForces();
}
void CalSpringSystem::update(float deltaTime)
{
// get the attached meshes vector
std::vector<CalMesh *>& vectorMesh = m_pModel->getVectorMesh();
// loop through all the attached meshes
std::vector<CalMesh *>::iterator iteratorMesh;
for(iteratorMesh = vectorMesh.begin(); iteratorMesh != vectorMesh.end(); ++iteratorMesh)
{
// get the ssubmesh vector of the mesh
std::vector<CalSubmesh *>& vectorSubmesh = (*iteratorMesh)->getVectorSubmesh();
// loop through all the submeshes of the mesh
std::vector<CalSubmesh *>::iterator iteratorSubmesh;
for(iteratorSubmesh = vectorSubmesh.begin(); iteratorSubmesh != vectorSubmesh.end(); ++iteratorSubmesh)
{
// check if the submesh contains a spring system
if((*iteratorSubmesh)->getCoreSubmesh()->getSpringCount() > 0 && (*iteratorSubmesh)->hasInternalData())
{
// calculate the new forces on each unbound vertex
calculateForces(*iteratorSubmesh, deltaTime);
// calculate the vertices influenced by the spring system
calculateVertices(*iteratorSubmesh, deltaTime);
}
}
}
}
//------------------------------------
void simulationManager::idle( float dt ){
for (int i = 0; i < NUM_PARTICLES; i++){
memcpy(PTStemp[i],PM->PTS[i],sizeof(particle));
PTStemp[i]->frc = 0;
}
// simulate the copy using different types of simulations
int simulation = 1;
switch (simulation){
case 0:
calculateForces();
calculateDerivatives();
addDerivativesToReal(dt);
break;
case 1:
calculateForces();
calculateDerivatives();
addDerivativesToTemp(0.5f,dt);
// reset the forces!
for (int i = 0; i < NUM_PARTICLES; i++){
PTStemp[i]->frc = 0;
}
calculateForces();
calculateDerivatives();
addDerivativesToReal(dt);
break;
}
}
void NormalModeLangLf::run(int numTimesteps) {
//Real h = getTimestep() * Constant::INV_TIMEFACTOR;
Real actTime;
if( numTimesteps < 1 )
return;
//check valid eigenvectors
if(*Q == NULL)
report << error << "No Eigenvectors for NormalMode integrator."<<endr;
//
//time calculated in forces! so fix here
actTime = app->topology->time + numTimesteps * getTimestep();
//
//main loop
for( int i = 0; i < numTimesteps; i++ ) {
//****main loop*************************************
preStepModify();
genProjGauss(&gaussRandCoord1, app->topology);
doHalfKick();
//
//nmlDrift(&app->positions, &app->velocities, h, app->topology);
doDrift();
//constraints?
app->energies.clear();
//run minimizer if any remaining modes
if(testRemainingModes()) myNextIntegrator->run(myCycleLength); //cyclelength
if(app->eigenInfo.reDiagonalize){ //rediagonalize?
app->topology->time = actTime + (i - numTimesteps) * getTimestep();
if(myPreviousIntegrator == NULL)
report << error << "[NormalModeLangLf::Run] Re-diagonalization forced with NormalModeLangLf as outermost Integrator. Aborting."<<endr;
return;
}
//calculate sub space forces
app->energies.clear();
calculateForces();
//
genProjGauss(&gaussRandCoord1, app->topology);
doHalfKick();
//
postStepModify();
}
//fix time
app->topology->time = actTime;
//
}
void CSimulation::next_step()
{
check_for_start();
//std::cout << "RUNNING WITH BOOST SEED AND RAND SEED 1" << std::endl;
//std::cout << "t: " << t << std::endl;
if (is_paused()) return;
calculateFinishTime();
calculateForces();
t++;
check_for_end();
}
void calculateAcceleration(){
calculateNewGravityVec();
// Clear all particles in cells
for (int j = 0; j < Cell::GRID_WIDTH * Cell::GRID_HEIGHT; j++) {
cells[j].clearParticles();
}
// Push every particle into corresponding cell
for(int i = 0; i < NUM_PARTICLES; i++) {
cells[particles[i].getCellIndex()].addParticle(particles[i]);
}
calculateDensityAndPressure();
calculateForces();
}
void NormalModeMinimizer::utilityCalculateForces(){
calculateForces();
}
void NormalModeRelax::utilityCalculateForces(){
app->energies.clear(); //need this as MTS, not innermost
calculateForces();
}
//------------------------------------------------------------------------------
// Adaptive Dynamic relaxation to solve the quasi-static problem.
//------------------------------------------------------------------------------
void AdaptiveDynamicRelaxation::stepForward(int i, double t)
{
(void) i;
(void) t;
globalError = 0;
double alpha = (2*dt)/(2 + c*dt);
double beta = (2-c*dt)/(2 + c*dt);
double gErr = 0;
// Updating the velocityL
#ifdef _OPENMP
# pragma omp parallel for schedule(runtime) reduction(max: gErr)
#endif
for(int i=0;i<domain->myGrid.size(); i++)
{
VEC3 error;
double maxE = 0.0;
int gridId = domain->myGrid[i];
GridPoint * gridPoint = domain->grid[gridId];
for(Particle * p:gridPoint->getParticles())
{
int p_id = p->id();
p->updateState();
p->zeroStress();
VEC3 & r = p->r();
VEC3 & r0 = p->r0();
VEC3 & v = p->v();
VEC3 & f = p->f();
for(int d=0; d<dim; d++){
error[d] = r[d];
v[d] = beta*v[d] + alpha/stableMass[p_id*dim + d]*f[d];
r[d] += dt*v[d];
Fold[p_id*dim + d] = f[d];
f[d] = 0;
// Error calculation
error[d] = std::abs(r[d] - error[d]);
maxE = std::max(maxE, std::abs(r[d] - r0[d]));
}
// Max error
double mError = 0;
if(maxE > 0)
{
for(int d=0; d<dim; d++){
error[d] = error[d]/maxE;
mError = std::max(mError, error[d]);
}
}
gErr = std::max(mError, gErr);
}
}
globalError = gErr;
if(i % verletListUpdateTime == 0)
createVerletList();
// cout << "c = " << c << " " << gErr << endl;
calculateForces();
domain->update();
// Updating modifiers
for(Modifier *modifier:modifiers)
{
modifier->evaluateAfterForceCalculation(t, i);
}
// Calculating the damping coefficient
double numerator = 0;
double denominator = 0;
#ifdef _OPENMP
# pragma omp parallel for schedule(runtime)
#endif
for(int i=0;i<domain->myGrid.size(); i++)
{
int gridId = domain->myGrid[i];
GridPoint * gridPoint = domain->grid[gridId];
for(Particle * p:gridPoint->getParticles())
{
int p_id = p->id();
const VEC3 & f = p->f();
const VEC3 & v = p->v();
for(int d=0; d<dim; d++)
{
numerator += -v[d]*(f[d] - Fold[p_id*dim + d])/(stableMass[p_id*dim + d]*dt);
denominator += v[d]*v[d];
}
}
}
c = 0;
if(denominator > 0)
{
if(numerator/denominator > 0)
{
c = 2*sqrt(numerator/denominator);
}
}
// if(c > 2)
// c = 1.9;
}
long NormalModeMori::run(const long numTimesteps) {
Real h = getTimestep() * Constant::INV_TIMEFACTOR;
Real actTime;
if( numTimesteps < 1 ) return 0;
//check valid eigenvectors
if(*Q == NULL)
report << error << "No Eigenvectors for NormalMode integrator."<<endr;
//
//time calculated in forces! so fix here
actTime = app->topology->time + numTimesteps * getTimestep();
//
//main loop
for( int i = 0; i < numTimesteps; i++ ) {
//****main loop*************************************
preStepModify();
doHalfKick();
//
nmlDrift(&app->positions, &app->velocities, h, app->topology);
//constraints?
app->energies.clear();
//run minimizer if any remaining modes
if(testRemainingModes()) myNextIntegrator->run(myCycleLength); //cyclelength
if(*Q == NULL){ //rediagonalize?
app->topology->time = actTime - (i - numTimesteps) * getTimestep();
if(myPreviousIntegrator == NULL)
report << error << "[NormalModeMori::Run] Re-diagonalization forced with NormalModeMori as outermost Integrator. Aborting."<<endr;
return i;
}
//#################Put averaged force code here##############################
//calculate sub space forces, just do this for the energy
Real minPotEnergy = (app->energies)[ScalarStructure::OTHER]; //save minimum potential energy
app->energies.clear();
calculateForces();
//transfer the real average force, Note: force fields must be identical
if(!instForce){
for( unsigned int i=0;i < app->positions.size(); i++) (*myForces)[i] = myBottomNormalMode->tempV3DBlk[i];
}
(app->energies)[ScalarStructure::OTHER] = minPotEnergy; //restore minimum potential energy
//###########################################################################
//
doHalfKick();
//
postStepModify();
}
//####diagnostics
//output modes?
if(modeOutput != ""){
//get modes
modeSpaceProj(tmpC, &app->positions, ex0);
//Output modes for analysis
ofstream myFile;
myFile.open(modeOutput.c_str(),ofstream::app);
myFile.precision(10);
for(int ii=0;ii<_rfM-1;ii++) myFile << tmpC[ii] << ", ";
myFile << tmpC[_rfM-1] << endl; //last
//close file
myFile.close();
}
//####
//fix time
app->topology->time = actTime;
return numTimesteps;
}
void telemetryCallback(const AU_UAV_ROS::TelemetryUpdate::ConstPtr &msg)
{
AU_UAV_ROS::GoToWaypoint goToWaypointSrv;
AU_UAV_ROS::RequestWaypointInfo requestWaypointInfoSrv;
int planeID = msg->planeID;
/* request this plane's current normal destination */
requestWaypointInfoSrv.request.planeID = planeID;
requestWaypointInfoSrv.request.isAvoidanceWaypoint = false;
requestWaypointInfoSrv.request.positionInQueue = 0;
if (!requestWaypointInfoClient.call(requestWaypointInfoSrv)){
ROS_ERROR("Did not receive a response from the coordinator");
return;
}
destLatArray[planeID] = requestWaypointInfoSrv.response.latitude;
destLonArray[planeID] = requestWaypointInfoSrv.response.longitude;
// check for the total number of planes present to use the correct parameters
if (planeID+1 > maxPlanesFound) maxPlanesFound++;
// check the positions of all the waypoints and planes to see the field size for the correct parameters
// it IS possible for a larger scenario to seem like a smaller scenario by chance, but that should be okay
if (planeID == maxPlanesFound-1)
{
fieldSize = 500;
for (unsigned int i = 0; i < maxPlanesFound; i += 1)
{
// check waypoints
if (destLatArray[i] < (SOUTH_MOST_LATITUDE_500M - LATITUDE_EPSILON) ||
destLonArray[i] > (EAST_MOST_LONGITUDE_500M + LONGITUDE_EPSILON))
{
fieldSize = 1000;
break;
}
}
// use the plane and waypoint information to find the correct parameters (update once per cycle)
if (fieldSize == 500){
if (maxPlanesFound <= 4){
setupVariables(param_4plane_500m);
}
else if (maxPlanesFound <= 8){
setupVariables(param_8plane_500m);
}
else if (maxPlanesFound <= 16){
setupVariables(param_16plane_500m);
}
else if (maxPlanesFound <= 32){
setupVariables(param_32plane_500m);
}
else
{
std::cout << "plane number error: more than 32 UAVs found- using 32 plane case" << std::endl;
setupVariables(param_32plane_500m);
}
}
else if (fieldSize == 1000){
if (maxPlanesFound <= 4){
setupVariables(param_4plane_1000m);
}
else if (maxPlanesFound <= 8){
setupVariables(param_8plane_1000m);
}
else if (maxPlanesFound <= 16){
setupVariables(param_16plane_1000m);
}
else if (maxPlanesFound <= 32){
setupVariables(param_32plane_1000m);
}
else
{
setupVariables(param_32plane_1000m);
std::cout << "plane number error: more than 32 UAVs found- using 32 plane case" << std::endl;
}
}
else std::cout << "field size error: invalid field size determined (this should never happen)" << std::endl;
std::cout << "Using parameters for a " << fieldSize << "m field and " << maxPlanesFound << " UAVs" << std::endl;
}
/*
Remove any planes that have been involved in a collision.
Note: This function is made for use with the evaluator node, and may not work optimally in the field.
To check for collisions, it compares the current time with the last update time of each of the UAVs.
If the values differ by more than three seconds, it is assumed the plane has been deleted. However,
packet losses / network latency may render issues in the real world.
*/
checkCollisions();
/*
if (plane has reached current destination waypoint)
move on to next normal destination waypoint in queue
*/
if (findDistance(msg->currentLatitude, msg->currentLongitude,
requestWaypointInfoSrv.response.latitude,
requestWaypointInfoSrv.response.longitude) < COLLISION_THRESHOLD){
/* request next normal destination */
requestWaypointInfoSrv.request.positionInQueue = 1;
if (!requestWaypointInfoClient.call(requestWaypointInfoSrv)){
ROS_ERROR("Did not recieve a response from the coordinator");
return;
}
}
/*
Pseudocode for the following code lines.
if (this plane is not in the map of planes and the telemetry update indicates that the plane is or has been moving towards a destination)
if (the plane had been flying but we previously detected a collision and removed it)
return - the plane is dead; the simulator is lagging behind
else
the plane has registered a new TelemetryUpdate
else
return - the plane has just been initialized but it is not moving torwards a waypoint as of now
*/
if (pobjects.find(planeID) == pobjects.end() && msg->currentWaypointIndex != -1){
if (msg->currentWaypointIndex > 0){
/* This plane is dead; it had previously been moving but was in a collision.
The simulator is lagging behind and still reporting a telemetry update */
return;
}
else{
/*
a new plane has registered a TelemetryUpdate where the destination is not (0, 0)
create new PlaneObject, collision radius is set to the distance traveled in one second
*/
AU_UAV_ROS::PlaneObject newPlane(MPS_SPEED, *msg); /* */
pobjects[planeID] = newPlane; /* put the new plane into the map */
/* update the destination of the PlaneObject with the value found with the requestWaypointInfoSrv call */
AU_UAV_ROS::waypoint newDest;
newDest.latitude = requestWaypointInfoSrv.response.latitude;
newDest.longitude = requestWaypointInfoSrv.response.longitude;
newDest.altitude = requestWaypointInfoSrv.response.altitude;
pobjects[planeID].setDestination(newDest);
}
}
else if (pobjects.find(planeID) == pobjects.end()) /* new plane without waypoint set */
return;
/*
Note: The requestWaypointInfo service returns a waypoint of -1000, -1000 when the UAV it cannot retrieve a destination from
queue.
Pseudocode:
if (the plane has no destination){
- for simulations, silence any force from this UAV so it does not affect flight paths by giving it an impossible location
- update with the new time of latest update to avoid a false detection of a collision
}
else{
update the plane with the new telemetry information
if (the plane's destination has changed)
update the map entry of the plane with this information
}
*/
if (requestWaypointInfoSrv.response.latitude == -1000){ /* plane has no waypoints to go to */
/*
useful for simulations, remove in real flights;
set location of finished planes to -1000, -1000 so no repulsive force is felt from this plane
*/
pobjects[planeID].setLatitude(-1000);
pobjects[planeID].setLongitude(-1000);
pobjects[planeID].update(); /* update the time of last update for this plane to acknowledge it is still in the air */
return;
}
else{
pobjects[planeID].update(*msg); /* update plane with new position */
/* if (destination has changed)
update pobjects[planeID] with new destination */
if (((pobjects[planeID].getDestination().latitude - requestWaypointInfoSrv.response.latitude) > EPSILON)
|| ((pobjects[planeID].getDestination().longitude - requestWaypointInfoSrv.response.longitude) > EPSILON)
|| ((pobjects[planeID].getDestination().latitude - requestWaypointInfoSrv.response.latitude) < EPSILON)
|| ((pobjects[planeID].getDestination().longitude - requestWaypointInfoSrv.response.longitude) < EPSILON)){
AU_UAV_ROS::waypoint newDest;
newDest.latitude = requestWaypointInfoSrv.response.latitude;
newDest.longitude = requestWaypointInfoSrv.response.longitude;
newDest.altitude = requestWaypointInfoSrv.response.altitude;
pobjects[planeID].setDestination(newDest);
}
}
//form flocks
formFlocks();
//organize flock information
checkFlocks();
// give the UAVs their flock numbers
for (unsigned int i = 0; i < flocks.size(); i += 1)
{
for (unsigned int j = 0; j < flocks[i].size(); j += 1)
{
flocks[i][j]->setFlock(i);
}
}
/* Find the force acting on this UAV. The plane is attracted to its waypoint or leader, and repelled from other UAVs */
AU_UAV_ROS::mathVector force = calculateForces(pobjects[planeID], pobjects, flocks, forceVars);
/*
Create a goToWaypoint service to send to the coordinator based on the force vector just calculated. The destination will be one
second away from the current position.
*/
goToWaypointSrv = updatePath(msg, force);
goToWaypointSrv.request.isAvoidanceManeuver = true;
goToWaypointSrv.request.isNewQueue = true;
if (goToWaypointClient.call(goToWaypointSrv)){
count++;
//ROS_INFO("Received response from service request %d", (count-1));
}
else{
ROS_ERROR("Did not receive response");
}
}
void Data::updateWithHydro(const std::vector &vector, const Boundaries &boundaries) {
calculateForces();
calculateBaseNoise();
}
