C++ PlaneObject类代码示例

/// \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;
}

/* 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;
}