/// \param dt Change in time
void StatePlaying::processCamera(float dt)
{
PlaneObject *planeObject = m_objects->getPlaneObject();
Camera * cam = gGame.m_currentCam;
// this should never happen but if it does leave
if (planeObject == NULL || cam == NULL)
return;
// Grab current states
Vector3 cameraPosOld = cam->cameraPos;
Vector3 planePosOld = planeObject->getPosition();
cam->process(dt); // move camera
// Test for camera collision with terrain
const float CameraHeight = 2.0f;
float fTerrainHt = terrain->getHeight(cam->cameraPos.x,cam->cameraPos.z);
if(cam->cameraPos.y - fTerrainHt < CameraHeight)
cam->cameraPos.y = fTerrainHt + CameraHeight;
// location is now above terrain, set it
cam->setAsCamera();
// Tell terrain where the camera is so that it can adjust for LOD
terrain->setCameraPos(cam->cameraPos);
// Set 3D sound parameters based on new camera position and velocity
Vector3 cameraVel = (cam->cameraPos - cameraPosOld) / dt;
gSoundManager.setListenerPosition(cam->cameraPos);
gSoundManager.setListenerVelocity(cameraVel);
gSoundManager.setListenerOrientation(cam->cameraOrient);
}
void Game::renderScreen()
{
m_currentCam->setAsCamera();
renderScene(false);
PlaneObject *plane = objects->getPlaneObject();
if(plane != NULL)
{
//render plane reticles
plane->renderReticle(15.0f, 1.3f);
plane->renderReticle(8.0f, 1.0f);
if (plane->isPlaneAlive() == false)
{
int textY = gRenderer.getScreenY()/2;
IRectangle rect = IRectangle(0,textY,gRenderer.getScreenX()-1, textY + 30);
gRenderer.drawText("Press \"Space Bar\" to Respawn",&rect, eTextAlignModeCenter, false);
}
}
gConsole.render();
if (GameBase::m_renderInfo)
GameBase::renderInfo();
}
/* This function calculates any maneuvers that are necessary for the
current plane to avoid looping. Returns a waypoint based on calculations.
If no maneuvers are necessary, then the function returns the current
destination. */
sim::waypoint sim::takeDubinsPath(PlaneObject &plane1) {
/* Initialize variables */
sim::coordinate circleCenter;
sim::waypoint wp = plane1.getDestination();
double minTurningRadius = 0.75*MINIMUM_TURNING_RADIUS;
bool destOnRight;
/* Calculate cartesian angle from plane to waypoint */
double wpBearing = findAngle(plane1.getCurrentLoc().latitude,
plane1.getCurrentLoc().longitude, wp.latitude, wp.longitude);
/* Calculate cartesian current bearing of plane (currentBearing is stored as Cardinal) */
double currentBearingCardinal = plane1.getCurrentBearing();
double currentBearingCartesian = toCartesian(currentBearingCardinal);
if (fabs(currentBearingCardinal) < 90.0)
/* Figure out which side of the plane the waypoint is on */
destOnRight = ((wpBearing < currentBearingCartesian) &&
(wpBearing > manipulateAngle(currentBearingCartesian - 180.0)));
else
destOnRight = !((wpBearing > currentBearingCartesian) &&
(wpBearing < manipulateAngle(currentBearingCartesian - 180.0)));
/* Calculate the center of the circle of minimum turning radius on the side that the waypoint is on */
circleCenter = calculateLoopingCircleCenter(plane1, minTurningRadius, destOnRight);
/* If destination is inside circle, must fly opposite direction before we can reach destination */
if (findDistance(circleCenter.latitude, circleCenter.longitude, wp.latitude, wp.longitude) <
minTurningRadius)
return calculateWaypoint(plane1, minTurningRadius, !destOnRight);
else
return wp;
}
/* Find the new collision avoidance waypoint for the plane to go to */
sim::waypoint sim::calculateWaypoint(PlaneObject &plane1, double turningRadius, bool turnRight){
sim::waypoint wp;
double V = MPS_SPEED;
double delta_T = TIME_STEP;
double cartBearing = toCartesian(plane1.getCurrentBearing())* PI/180;
double delta_psi = V / turningRadius * delta_T;
if (turnRight) delta_psi *= -1.0;
double psi = (cartBearing + delta_psi);
wp.longitude = plane1.getCurrentLoc().longitude + V*cos(psi)/DELTA_LON_TO_METERS;
wp.latitude = plane1.getCurrentLoc().latitude + V*sin(psi)/DELTA_LAT_TO_METERS;
wp.altitude = plane1.getCurrentLoc().altitude;
return wp;
}
AU_UAV_ROS::coordinate AU_UAV_ROS::calculateLoopingCircleCenter(PlaneObject &plane, double turnRadius, bool turnRight) {
AU_UAV_ROS::coordinate circleCenter;
circleCenter.altitude = plane.getCurrentLoc().altitude;
double angle;
if (turnRight) {
angle = (toCartesian(plane.getCurrentBearing()) - 90 - 22.5) * PI/180.0;
}
else {
angle = (toCartesian(plane.getCurrentBearing()) + 90 + 22.5) * PI/180.0;
}
double xdiff = turnRadius*cos(angle);
double ydiff = turnRadius*sin(angle);
circleCenter.longitude = plane.getCurrentLoc().longitude + xdiff/DELTA_LON_TO_METERS;
circleCenter.latitude = plane.getCurrentLoc().latitude + ydiff/DELTA_LAT_TO_METERS;
return circleCenter;
}
void OptiXRenderer::convertToOptiXScene(optix::Context context, int width, int height, float film_location) {
// Setup lighting
// TODO For now, we assume just one light
context->setEntryPointCount( 1 );
context->setRayGenerationProgram( 0, mScene->mLights[0]->getOptiXLight(context) );
// Exception program
context->setExceptionProgram( 0, context->createProgramFromPTXFile( "ptx/PhotonTracer.ptx", "exception" ) );
// Miss program
context->setMissProgram( 0, context->createProgramFromPTXFile( "ptx/PhotonTracer.ptx", "miss" ) );
// Geometry group
optix::GeometryGroup geometrygroup = context->createGeometryGroup();
geometrygroup->setChildCount( mScene->mObjects.size() + 1 );
geometrygroup->setAcceleration( context->createAcceleration("Bvh","Bvh") );
// Add objects
for(std::vector<RenderObject*>::size_type i = 0; i != mScene->mObjects.size(); i++) {
optix::GeometryInstance gi = context->createGeometryInstance();
// TODO we only support 1 material type per object
gi->setMaterialCount(1);
gi->setGeometry( mScene->mObjects[i]->getOptiXGeometry(context));
gi->setMaterial(0, mScene->mObjects[i]->getOptiXMaterial(context));
geometrygroup->setChild(i, gi);
}
// Create Camera
//
mCameraMat = new CameraMaterial(width, height, 1);
PlaneObject* plane = new PlaneObject(mCameraMat);
plane->setPosition(0, 0, film_location); //-65
mCameraObject = plane;
// Convert to OptiX
optix::GeometryInstance gi = context->createGeometryInstance();
gi->setMaterialCount(1);
gi->setGeometry( mCameraObject->getOptiXGeometry(context));
mCameraMatOptiX = mCameraObject->getOptiXMaterial(context);
mCameraMatOptiX["width"]->setInt(width);
mCameraMatOptiX["height"]->setInt(height);
gi->setMaterial(0, mCameraMatOptiX);
geometrygroup->setChild(mScene->mObjects.size(), gi);
context["top_object"]->set(geometrygroup);
}
/* Find the new collision avoidance waypoint for the plane to go to */
AU_UAV_ROS::waypoint AU_UAV_ROS::calculateWaypoint(PlaneObject &plane1, double turningRadius, bool turnRight){
AU_UAV_ROS::waypoint wp;
double V = MPS_SPEED;
double delta_T = TIME_STEP;
double cartBearing = toCartesian(plane1.getCurrentBearing())* PI/180;
double delta_psi = V / turningRadius * delta_T;
if (turnRight) delta_psi *= -1.0;
ROS_WARN("Delta psi: %f", delta_psi);
double psi = (cartBearing + delta_psi);
wp.longitude = plane1.getCurrentLoc().longitude + V*cos(psi)/DELTA_LON_TO_METERS;
wp.latitude = plane1.getCurrentLoc().latitude + V*sin(psi)/DELTA_LAT_TO_METERS;
wp.altitude = plane1.getCurrentLoc().altitude;
ROS_WARN("Plane%d new cbearing: %f", plane1.getID(), toCardinal( findAngle(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude, wp.latitude, wp.longitude) ) );
//ROS_WARN("Plane%d calc lat: %f lon: %f w/ act lat: %f lon: %f", plane1.getID(), wp.latitude, wp.longitude, plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
return wp;
}
void StatePlaying::processInput()
{
PlaneObject *planeObject = m_objects->getPlaneObject();
// Exit to menu if requested
if (gInput.keyJustUp(DIK_ESCAPE, true))
{
gGame.changeState(eGameStateMenu);
return;
}
// If you press space bar after you die reset game
if (gInput.keyJustUp(DIK_SPACE))
if (planeObject->isPlaneAlive() == false)
{
resetGame();
return;
}
}
bool Ned3DObjectManager::interactPlaneCrow(PlaneObject &plane, CrowObject &crow)
{
bool collided = enforcePositions(plane, crow);
if(collided && !crow.isDying())
{
shootCrow(crow);
plane.damage(1);
}
return collided;
}
/* Returns true if the original plane (plane1) should turn right to avoid plane2,
false if otherwise. Takes original plane and its greatest threat as parameters. */
bool sim::shouldTurnRight(PlaneObject &plane1, PlaneObject &plane2) {
/* For checking whether the plane should turn right or left */
double theta, theta_dot, R;
double cartBearing1 = toCartesian(plane1.getCurrentBearing());
double cartBearing2 = toCartesian(plane2.getCurrentBearing());
double V = MPS_SPEED;
/* Calculate theta, theta1, and theta2. Theta is the cartesian angle
from 0 degrees (due East) to plane2 (using plane1 as the origin). This
may be referred to as the LOS angle. */
theta = findAngle(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
R = findDistance(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
theta_dot = (V*sin((cartBearing2 - theta)*PI/180) - V*sin((cartBearing1 - theta)*PI/180)) / R;
return (theta_dot >= 0);
}
bool Ned3DObjectManager::interactPlaneEnemy(PlaneObject &plane, EnemyObject &enemy)
{
bool collided = enforcePositions(plane, enemy);
if(collided && !enemy.isDying())
{
shootEnemy(enemy);
plane.damage(1);
}
return collided;
}
void StatePlaying::renderScreen()
{
// render the entire scene
renderScene();
PlaneObject* plane = m_objects->getPlaneObject();
if (plane != NULL)
{
//render plane reticles
plane->renderReticle(15.0f, 1.3f);
plane->renderReticle(8.0f, 1.0f);
if (plane->isPlaneAlive() == false)
{
int textY = gRenderer.getScreenY()/2;
IRectangle rect = IRectangle(0,textY,gRenderer.getScreenX()-1, textY + 30);
gRenderer.drawText("Press \"Space Bar\" to Respawn",&rect, eTextAlignModeCenter, false);
}
}
// render FPS and console ontop of everything
gGame.GameBase::renderConsoleAndFPS();
}
/* Find the new collision avoidance waypoint for the plane to go to */
au_uav_ros::waypoint au_uav_ros::calculateWaypoint(PlaneObject &plane1, double turningRadius, bool turnRight){
au_uav_ros::waypoint wp;
double V = MPS_SPEED * MPS_WAYPOINT_MULTIPLIER;
double delta_T = TIME_STEP;
double cartBearing = plane1.getCurrentBearing()* PI/180;
double delta_psi = V / turningRadius * delta_T;
if (turnRight) delta_psi *= -1.0;
ROS_WARN("Delta psi: %f", delta_psi);
double psi = (cartBearing + delta_psi);
V = V * MPS_WAYPOINT_MULTIPLIER;
wp.longitude = plane1.getCurrentLoc().longitude + V*cos(psi)/DELTA_LON_TO_METERS;
wp.latitude = plane1.getCurrentLoc().latitude + V*sin(psi)/DELTA_LAT_TO_METERS;
ROS_INFO("long+%f, lat+%f, distanceBetween UAV and AvoidWP%f", V*cos(psi)/DELTA_LON_TO_METERS, V*sin(psi)/DELTA_LON_TO_METERS,
distanceBetween(plane1.getCurrentLoc(), wp));
wp.altitude = plane1.getCurrentLoc().altitude;
ROS_WARN("Plane%d new cbearing: %f", plane1.getID(), toCardinal( findAngle(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude, wp.latitude, wp.longitude) ) );
//ROS_WARN("Plane%d calc lat: %f lon: %f w/ act lat: %f lon: %f", plane1.getID(), wp.latitude, wp.longitude, plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
return wp;
}
bool Ned3DObjectManager::interactPlaneWater(PlaneObject &plane, WaterObject &water)
{
Water *pWater = water.getWater();
if(pWater == NULL) return false;
// Test for plane collision with water
Vector3 planePos = plane.getPosition();
EulerAngles planeOrient = plane.getOrientation();
Vector3 disp = planePos - disp;
RotationMatrix planeMatrix;
planeMatrix.setup(plane.getOrientation()); // get plane's orientation
float planeBottom = plane.getBoundingBox().min.y;
float waterHeight = pWater->getWaterHeight();
if(plane.isPlaneAlive() && planeBottom < waterHeight)
{ //collision
Vector3 viewVector = planeMatrix.objectToInertial(Vector3(0,0,1));
plane.killPlane();
plane.setSpeed(0.0f);
planePos += 2.0f * viewVector;
planeOrient.pitch = kPi / 4.0f;
planeOrient.bank = kPi / 4.0f;
plane.setOrientation(planeOrient);
plane.setPPosition(planePos);
int partHndl = gParticle.createSystem("planeexplosion");
gParticle.setSystemPos(partHndl, plane.getPosition());
int boomHndl = gSoundManager.requestSoundHandle("Boom.wav");
int boomInst = gSoundManager.requestInstance(boomHndl);
if(boomInst != SoundManager::NOINSTANCE)
{
gSoundManager.setPosition(boomHndl,boomInst,plane.getPosition());
gSoundManager.play(boomHndl,boomInst);
gSoundManager.releaseInstance(boomHndl,boomInst);
}
return true;
}
return false;
}
bool Ned3DObjectManager::interactPlaneTerrain(PlaneObject &plane, TerrainObject &terrain)
{
Terrain *terr = terrain.getTerrain();
if(terr == NULL) return false;
//test for plane collision with terrain
Vector3 planePos = plane.getPosition();
EulerAngles planeOrient = plane.getOrientation();
Vector3 disp = planePos - disp;
RotationMatrix planeMatrix;
planeMatrix.setup(plane.getOrientation()); // get plane's orientation
float planeBottom = plane.getBoundingBox().min.y;
float terrainHeight = terr->getHeight(planePos.x,planePos.z);
if(plane.isPlaneAlive() && planeBottom < terrainHeight)
{ //collision
Vector3 viewVector = planeMatrix.objectToInertial(Vector3(0,0,1));
if(viewVector * terr->getNormal(planePos.x,planePos.z) < -0.5f // dot product
|| plane.isCrashing())
{
plane.killPlane();
int partHndl = gParticle.createSystem("planeexplosion");
gParticle.setSystemPos(partHndl, plane.getPosition());
int boomHndl = gSoundManager.requestSoundHandle("Boom.wav");
int boomInst = gSoundManager.requestInstance(boomHndl);
if(boomInst != SoundManager::NOINSTANCE)
{
gSoundManager.setPosition(boomHndl,boomInst,plane.getPosition());
gSoundManager.play(boomHndl,boomInst);
gSoundManager.releaseInstance(boomHndl,boomInst);
}
plane.setSpeed(0.0f);
planePos += 2.0f * viewVector;
planeOrient.pitch = kPi / 4.0f;
planeOrient.bank = kPi / 4.0f;
plane.setOrientation(planeOrient);
}
else planePos.y = terrainHeight + planePos.y - planeBottom;
//plane.setPPosition(planePos);
return true;
}
return false;
}
/* Function that returns the ID of the most dangerous neighboring plane and its ZEM */
AU_UAV_ROS::threatContainer AU_UAV_ROS::findGreatestThreat(PlaneObject &plane1, std::map<int, PlaneObject> &planes){
/* Set reference for origin (Northwest corner of the course)*/
AU_UAV_ROS::coordinate origin;
origin.latitude = 32.606573;
origin.longitude = -85.490356;
origin.altitude = 400;
/* Set preliminary plane to avoid as non-existent and most dangerous
ZEM as negative*/
int planeToAvoid = -1;
double mostDangerousZEM = -1;
/* Set the preliminary time-to-go to infinity*/
double minimumTimeToGo = std::numeric_limits<double>::infinity();
/* Declare second plane and ID variable */
PlaneObject plane2;
int ID;
/* Make a position vector representation of the current plane*/
double magnitude2, direction2;
double magnitude = findDistance(origin.latitude, origin.longitude,
plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
double direction = findAngle(origin.latitude, origin.longitude,
plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
AU_UAV_ROS::mathVector p1(magnitude,direction);
/* Make a heading vector representation of the current plane*/
AU_UAV_ROS::mathVector d1(1.0,toCartesian(plane1.getCurrentBearing()));
/* Declare variables needed for this loop*/
AU_UAV_ROS::mathVector pDiff;
AU_UAV_ROS::mathVector dDiff;
double timeToGo, zeroEffortMiss, distanceBetween, timeToDest;
std::map<int,AU_UAV_ROS::PlaneObject>::iterator it;
for ( it=planes.begin() ; it!= planes.end(); it++ ){
/* Unpacking plane to check*/
ID = (*it).first;
plane2 = (*it).second;
/* If it's not in the Check Zone, check the other plane*/
distanceBetween = plane1.findDistance(plane2);
if (distanceBetween > CHECK_ZONE || plane1.getID() == ID) continue;
else if (distanceBetween < MPS_SPEED) {
planeToAvoid = ID;
mostDangerousZEM = 0;
minimumTimeToGo = 0.1;
break;
}
/* Making a position vector representation of plane2*/
magnitude2 = findDistance(origin.latitude, origin.longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
direction2 = findAngle(origin.latitude, origin.longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
AU_UAV_ROS::mathVector p2(magnitude2,direction2);
/* Make a heading vector representation of the current plane*/
AU_UAV_ROS::mathVector d2(1.0,toCartesian(plane2.getCurrentBearing()));
/* Compute Time To Go*/
pDiff = p1-p2;
dDiff = d1-d2;
timeToGo = -1*pDiff.dotProduct(dDiff)/(MPS_SPEED*dDiff.dotProduct(dDiff));
/* Compute Zero Effort Miss*/
zeroEffortMiss = sqrt(pDiff.dotProduct(pDiff) +
2*(MPS_SPEED*timeToGo)*pDiff.dotProduct(dDiff) +
pow(MPS_SPEED*timeToGo,2)*dDiff.dotProduct(dDiff));
/* If the Zero Effort Miss is less than the minimum required
separation, and the time to go is the least so far, then avoid this plane*/
if(zeroEffortMiss <= DANGER_ZEM && timeToGo < minimumTimeToGo && timeToGo > 0){
// If the plane is behind you, don't avoid it
if ( fabs(plane2.findAngle(plane1)*180/PI - toCartesian(plane1.getCurrentBearing())) > 35.0) {
timeToDest = plane1.findDistance(plane1.getDestination().latitude,
plane1.getDestination().longitude) / MPS_SPEED;
/* If you're close to your destination and the other plane isn't
much of a threat, then don't avoid it */
if ( timeToDest < 5.0 && zeroEffortMiss > 3.0*MPS_SPEED ) continue;
planeToAvoid = ID;
mostDangerousZEM = zeroEffortMiss;
minimumTimeToGo = timeToGo;
}
}
}
AU_UAV_ROS::threatContainer greatestThreat;
greatestThreat.planeID = planeToAvoid;
greatestThreat.ZEM = mostDangerousZEM;
greatestThreat.timeToGo = minimumTimeToGo;
return greatestThreat;
}
/* This is the function called by collisionAvoidance.cpp that calls
all necessary functions in order to generate the new collision avoidance
waypoint. If no collision avoidance maneuvers are necessary, the function
returns the current destination waypoint. */
AU_UAV_ROS::waypointContainer AU_UAV_ROS::findNewWaypoint(PlaneObject &plane1, std::map<int, PlaneObject> &planes){
ROS_WARN("-----------------------------------------------------");
/* Find plane to avoid*/
AU_UAV_ROS::threatContainer greatestThreat = findGreatestThreat(plane1, planes);
/* Unpack plane to avoid*/
int threatID = greatestThreat.planeID;
double threatZEM = greatestThreat.ZEM;
double timeToGo = greatestThreat.timeToGo;
/*
if (threatID != -1) {
ROS_WARN("Distance between plane %d and plane %d = %f", plane1.getID(),
threatID, findDistance(plane1.getCurrentLoc().latitude,
plane1.getCurrentLoc().longitude, planes[threatID].getCurrentLoc().latitude,
planes[threatID].getCurrentLoc().longitude));
}
*/
AU_UAV_ROS::waypointContainer bothNewWaypoints;
/* If there is no plane to avoid, then take Dubin's path to the
destination waypoint*/
if (((threatID < 0) && (threatZEM < 0)) || timeToGo > MINIMUM_TIME_TO_GO) {
bothNewWaypoints.plane1WP = takeDubinsPath(plane1);
bothNewWaypoints.plane2ID = -1;
bothNewWaypoints.plane2WP = bothNewWaypoints.plane1WP;
return bothNewWaypoints;
}
/* If there is a plane to avoid, then figure out which direction it
should turn*/
bool turnRight = shouldTurnRight(plane1, planes[threatID]);
/* Calculate turning radius to avoid collision*/
double turningRadius = calculateTurningRadius(threatZEM);
/* Given turning radius and orientation of the plane, calculate
next collision avoidance waypoint*/
AU_UAV_ROS::waypoint plane1WP = calculateWaypoint(plane1, turningRadius, turnRight);
/* Cooperative planning*/
bothNewWaypoints.plane2ID = -1;
bothNewWaypoints.plane2WP = plane1WP;
if (findGreatestThreat(planes[threatID], planes).planeID == plane1.getID()) {
ROS_WARN("Planes %d and %d are each other's greatest threats", plane1.getID(), threatID);
ROS_WARN("Time to go = %f | ZEM = %f", timeToGo, threatZEM);
ROS_WARN("Plane %d bearing = %f | %d", plane1.getID(), plane1.getCurrentBearing(), turnRight);
ROS_WARN("Plane %d bearing = %f | %d", threatID, planes[threatID].getCurrentBearing(), turnRight);
AU_UAV_ROS::waypoint plane2WP = calculateWaypoint(planes[threatID], turningRadius, turnRight);
bothNewWaypoints.plane2WP = plane2WP;
bothNewWaypoints.plane2ID = threatID;
}
bothNewWaypoints.plane1WP = plane1WP;
return bothNewWaypoints;
}
/* Function that returns the ID of the most dangerous neighboring plane and its ZEM. */
sim::threatContainer sim::findGreatestThreat(PlaneObject &plane1, std::map<int, PlaneObject> &planes) {
/* Set reference for origin (Northwest corner of the course)*/
sim::coordinate origin;
origin.latitude = 32.606573;
origin.longitude = -85.490356;
origin.altitude = 400;
/* Set preliminary plane to avoid as non-existent and most dangerous ZEM as negative */
int planeToAvoid = -1;
int iPlaneToAvoid = -1;
double mostDangerousZEM = -1.0;
double iMostDangerousZEM = -1.0;
/* Set the preliminary time-to-go high */
double minimumTimeToGo = MINIMUM_TIME_TO_GO;
double iMinimumTimeToGo = 3.5;
/* Declare second plane and ID variable */
PlaneObject plane2;
int ID;
/* Make a position vector representation of the current plane */
double magnitude2, direction2;
double magnitude = findDistance(origin.latitude, origin.longitude,
plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
double direction = findAngle(origin.latitude, origin.longitude,
plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
sim::mathVector p1(magnitude,direction);
/* Make a heading vector representation of the current plane */
sim::mathVector d1(1.0,toCartesian(plane1.getCurrentBearing()));
/* Declare variables needed for this loop */
sim::mathVector pDiff, dDiff;
double timeToGo, zeroEffortMiss, distanceBetween, timeToDest, bearingDiff;
std::map<int,sim::PlaneObject>::iterator it;
for (it=planes.begin() ; it!= planes.end(); it++) {
/* Unpacking plane to check */
ID = (*it).first;
plane2 = (*it).second;
/* If it's not in the Check Zone, check the other plane */
distanceBetween = plane1.findDistance(plane2);
if (distanceBetween > CHECK_ZONE || plane1.getID() == ID) continue;
/* Making a position vector representation of plane2 */
magnitude2 = findDistance(origin.latitude, origin.longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
direction2 = findAngle(origin.latitude, origin.longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
sim::mathVector p2(magnitude2,direction2);
/* Make a heading vector representation of the other plane */
sim::mathVector d2(1.0,toCartesian(plane2.getCurrentBearing()));
/* Compute time-to-go */
pDiff = p1-p2;
dDiff = d1-d2;
timeToGo = -1.0*pDiff.dotProduct(dDiff)/(MPS_SPEED*dDiff.dotProduct(dDiff));
/* Compute Zero Effort Miss */
zeroEffortMiss = sqrt(fabs(pDiff.dotProduct(pDiff) +
2.0*(MPS_SPEED*timeToGo)*pDiff.dotProduct(dDiff) +
pow(MPS_SPEED*timeToGo,2.0)*dDiff.dotProduct(dDiff)));
if( zeroEffortMiss > DANGER_ZEM || (timeToGo > minimumTimeToGo && timeToGo > iMinimumTimeToGo) || timeToGo < 0 ) continue;
timeToDest = plane1.findDistance(plane1.getDestination().latitude,
plane1.getDestination().longitude) / MPS_SPEED;
/* If you're close to your destination and the other plane isn't
much of a threat, then don't avoid it */
if ( timeToDest < 5.0 && zeroEffortMiss > 3.0*MPS_SPEED ) continue;
/* If you're likely to zigzag, don't avoid the other plane */
bearingDiff = fabs(plane1.getCurrentBearing() - planes[ID].getCurrentBearing());
if ( plane1.findDistance(planes[ID]) > 3.5*MPS_SPEED && bearingDiff < CHATTERING_ANGLE) continue;
/* Second Threshold, to prevent planes from flying into others when trying to avoid less imminent collisions */
if ( zeroEffortMiss <= SECOND_THRESHOLD && timeToGo <= iMinimumTimeToGo ) {
iPlaneToAvoid = ID;
iMostDangerousZEM = zeroEffortMiss;
iMinimumTimeToGo = timeToGo;
continue;
}
planeToAvoid = ID;
mostDangerousZEM = zeroEffortMiss;
minimumTimeToGo = timeToGo;
}
sim::threatContainer greatestThreat;
if (iPlaneToAvoid > -1) {
greatestThreat.planeID = iPlaneToAvoid;
greatestThreat.ZEM = iMostDangerousZEM;
greatestThreat.timeToGo = iMinimumTimeToGo;
}
else {
greatestThreat.planeID = planeToAvoid;
greatestThreat.ZEM = mostDangerousZEM;
greatestThreat.timeToGo = minimumTimeToGo;
}
return greatestThreat;
}
void StatePlaying::process(float dt)
{
PlaneObject *planeObject = m_objects->getPlaneObject();
// this should never happen but if it does leave
if (planeObject == NULL)
return;
gConsole.process();
// call process and move on all objects in the object manager
m_objects->update(dt);
// process escape key and space bar
processInput();
// update location of camera
processCamera(dt);
// allow water to process per frame movements
water->process(dt);
// as soon as the plane crashes, start the timer
if (planeObject->isPlaneAlive() == false && m_planeCrashed == false)
{
m_planeCrashed = true; // set that the plane has crashed
m_timeSinceCrashed = 0.0f;
m_crowID = -1;
}
// once the timer hits 3 seconds, make the camera follow a crow
if (m_planeCrashed)
{
m_timeSinceCrashed += dt;
if (m_timeSinceCrashed >= 3.0f && m_crowID == -1)
{
// make the camera follow a crow around
m_crowID = m_objects->getCrow();
if (m_crowID != -1)
m_tetherCamera->setTargetObject(m_crowID);
// play the failed sound
m_failedSound = gSoundManager.requestSoundHandle("Failed.wav");
m_failedInstance = gSoundManager.requestInstance(m_failedSound);
if(m_failedInstance != SoundManager::NOINSTANCE)
{
gSoundManager.setToListener(m_failedSound,m_failedInstance);
gSoundManager.play(m_failedSound,m_failedInstance);
}
}
else if(m_failedInstance != SoundManager::NOINSTANCE)
gSoundManager.setToListener(m_failedSound,m_failedInstance);
}
// render reflection
if (Water::m_bReflection)
{
// render water reflection
Plane plane( 0, 1, 0, -water->getWaterHeight());
//get water texture ready
water->m_reflection.beginReflectedScene(plane);
renderScene(true);
water->m_reflection.endReflectedScene();
}
}
/*
Comparison function for two PlaneObject* parameteters. Returns a true if the first
object's distance to the destination is less than the second's, false otherwise.
*/
bool AU_UAV_ROS::cmpDistToDest(const PlaneObject &pobj1, const PlaneObject &pobj2){
return (findDistance(pobj1.getLatitude(), pobj1.getLongitude(), pobj1.getDestination().latitude, pobj1.getDestination().longitude) <
findDistance(pobj2.getLatitude(), pobj2.getLongitude(), pobj2.getDestination().latitude, pobj2.getDestination().longitude));
}
