自学教程：C++ AbstractVehicle类代码示例

//-----------------------------------------------------------------------------void EmptyPlugin::update (const float currentTime, const float elapsedTime){	if( false == this->isEnabled() )	{		return;	}	AbstractVehicleGroup kVehicles( m_kVehicles );	kVehicles.update( currentTime, elapsedTime );	if( 0 == m_bShowMotionStatePlot )	{		return;	}	AbstractVehicle* pVehicle = SimpleVehicle::getSelectedVehicle();	if( pVehicle != NULL )	{		// update motion state plot		this->m_kMotionStateProfile.recordUpdate( pVehicle, currentTime, elapsedTime );		m_pCamera->setCameraTarget( pVehicle );		m_pGrid->setGridCenter( pVehicle->position() );	}	BaseClass::update( currentTime, elapsedTime);}

float3OpenSteer::SteerLibrary::steerToAvoidCloseNeighbors (const AbstractVehicle& v, 							const float minSeparationDistance,                            const AVGroup& others){    // for each of the other vehicles...    for (AVIterator i = others.begin(); i != others.end(); i++)        {        AbstractVehicle& other = **i;        if (&other != &v)        {            const float sumOfRadii = v.radius() + other.radius();            const float minCenterToCenter = minSeparationDistance + sumOfRadii;            const float3 offset = float3_subtract(make_float3(other.position()), make_float3(v.position()));            const float currentDistance = float3_length(offset);            if (currentDistance < minCenterToCenter)            {                annotateAvoidCloseNeighbor (other, minSeparationDistance);				return float3_perpendicularComponent(float3_minus(offset), make_float3(v.forward()));            }        }    }    // otherwise return zero    return float3_zero();}

float3OpenSteer::SteerLibrary::steerForFlee (const AbstractVehicle& v, const float3& target){    const float3 desiredVelocity = float3_subtract(make_float3(v.position()), target);    return float3_subtract(desiredVelocity, v.velocity());}

floatOpenSteer::SteerLibrary::predictNearestApproachTime (const AbstractVehicle& v, 							const AbstractVehicle& other){    // imagine we are at the origin with no velocity,    // compute the relative velocity of the other vehicle    const float3 myVelocity = v.velocity();    const float3 otherVelocity = other.velocity();    const float3 relVelocity = float3_subtract(otherVelocity, myVelocity);    const float relSpeed = float3_length(relVelocity);    // for parallel paths, the vehicles will always be at the same distance,    // so return 0 (aka "now") since "there is no time like the present"    if (relSpeed == 0)		return 0;    // Now consider the path of the other vehicle in this relative    // space, a line defined by the relative position and velocity.    // The distance from the origin (our vehicle) to that line is    // the nearest approach.    // Take the unit tangent along the other vehicle's path	const float3 relTangent = float3_scalar_divide(relVelocity, relSpeed);    // find distance from its path to origin (compute offset from    // other to us, find length of projection onto path)    const float3 relPosition = float3_subtract(make_float3(v.position()), make_float3(other.position()));	const float projection = float3_dot(relTangent, relPosition);    return projection / relSpeed;}

float3OpenSteer::SteerLibrary::xxxsteerForSeek (const AbstractVehicle& v, const float3& target){    const float3 offset = float3_subtract(target, make_float3(v.position()));	const float3 desiredVelocity = float3_truncateLength(offset, v.maxSpeed());    return float3_subtract(desiredVelocity, v.velocity());}

void OpenSteer::OpenSteerDemo::circleHighlightVehicleUtility (const AbstractVehicle& vehicle){    if (&vehicle != NULL) drawXZCircle (vehicle.radius () * 1.1f,                                        vehicle.position(),                                        gGray60,                                        20);}

float3OpenSteer::SteerLibrary::steerForTargetSpeed (const AbstractVehicle& v, 					 const float targetSpeed){    const float mf = v.maxForce ();    const float speedError = targetSpeed - v.speed ();    return float3_scalar_multiply(make_float3(v.forward ()), clip (speedError, -mf, +mf));}

float3OpenSteer::SteerLibrary::steerToFollowPath (const AbstractVehicle& v, 				   const int direction,                   const float predictionTime,                   Pathway& path){    // our goal will be offset from our path distance by this amount    const float pathDistanceOffset = direction * predictionTime * v.speed();    // predict our future position    const float3 futurePosition = v.predictFuturePosition (predictionTime);    // measure distance along path of our current and predicted positions    const float nowPathDistance =        path.mapPointToPathDistance (make_float3(v.position ()));    const float futurePathDistance =        path.mapPointToPathDistance (futurePosition);    // are we facing in the correction direction?    const bool rightway = ((pathDistanceOffset > 0) ?                           (nowPathDistance < futurePathDistance) :                           (nowPathDistance > futurePathDistance));    // find the point on the path nearest the predicted future position    // XXX need to improve calling sequence, maybe change to return a    // XXX special path-defined object which includes two float3s and a     // XXX bool (onPath,tangent (ignored), withinPath)    float3 tangent;    float outside;    const float3 onPath = path.mapPointToPath (futurePosition,                                             // output arguments:                                             tangent,                                             outside);    // no steering is required if (a) our future position is inside    // the path tube and (b) we are facing in the correct direction    if ((outside < 0) && rightway)    {        // all is well, return zero steering        return float3_zero();    }    else    {        // otherwise we need to steer towards a target point obtained        // by adding pathDistanceOffset to our current path position        float targetPathDistance = nowPathDistance + pathDistanceOffset;        float3 target = path.mapPathDistanceToPoint (targetPathDistance);        annotatePathFollowing (futurePosition, onPath, target, outside);        // return steering to seek target on path        return steerForSeek (v, target);    }}

voidOpenSteer::SteerLibrary::findNextIntersectionWithSphere (const AbstractVehicle& v, 								SphericalObstacleData& obs,                                PathIntersection& intersection){    // xxx"SphericalObstacle& obs" should be "const SphericalObstacle&    // obs" but then it won't let me store a pointer to in inside the    // PathIntersection    // This routine is based on the Paul Bourke's derivation in:    //   Intersection of a Line and a Sphere (or circle)    //   http://www.swin.edu.au/astronomy/pbourke/geometry/sphereline/    float b, c, d, p, q, s;    float3 lc;    // initialize pathIntersection object    intersection.intersect = false;    intersection.obstacle = &obs;    // find "local center" (lc) of sphere in boid's coordinate space    lc = v.localizePosition (obs.center);    // computer line-sphere intersection parameters    b = -2 * lc.z;    c = square (lc.x) + square (lc.y) + square (lc.z) -         square (obs.radius + v.radius());    d = (b * b) - (4 * c);    // when the path does not intersect the sphere    if (d < 0) return;    // otherwise, the path intersects the sphere in two points with    // parametric coordinates of "p" and "q".    // (If "d" is zero the two points are coincident, the path is tangent)    s = sqrtXXX (d);    p = (-b + s) / 2;    q = (-b - s) / 2;    // both intersections are behind us, so no potential collisions    if ((p < 0) && (q < 0)) return;     // at least one intersection is in front of us    intersection.intersect = true;    intersection.distance =        ((p > 0) && (q > 0)) ?        // both intersections are in front of us, find nearest one        ((p < q) ? p : q) :        // otherwise only one intersections is in front, select it        ((p > 0) ? p : q);    return;}

void OpenSteer::OpenSteerDemo::position3dCamera (AbstractVehicle& selected,                                            float distance,                                            float /*elevation*/){    //selectedVehicle = &selected;    if (&selected)    {        const float3 behind = float3_scalar_multiply(make_float3(selected.forward()), -distance);        camera.setPosition ( make_float4(float3_add(make_float3(selected.position()), behind), 0.f) );        camera.target = make_float3(selected.position());    }}

void 	OpenSteer::OpenSteerDemo::position3dCamera (AbstractVehicle& selected,	float distance,	float /*elevation*/){	SteeringVehicle::setSelectedVehicle( &selected );	if (&selected)	{		const Vec3 behind = selected.forward() * -distance;		OpenSteer::Camera::accessInstance().setPosition (selected.position() + behind);		OpenSteer::Camera::accessInstance().target = selected.position();	}}

float3OpenSteer::SteerLibrary::steerForSeparation (const AbstractVehicle& v, 					const float maxDistance,                    const float cosMaxAngle,                    const AVGroup& flock){    // steering accumulator and count of neighbors, both initially zero    float3 steering = float3_zero();    int neighbors = 0;    // for each of the other vehicles...    for (AVIterator other = flock.begin(); other != flock.end(); other++)    {        if (inBoidNeighborhood (v, **other, v.radius() * 3, maxDistance, cosMaxAngle))        {            // add in steering contribution            // (opposite of the offset direction, divided once by distance            // to normalize, divided another time to get 1/d falloff)            const float3 offset = float3_subtract(make_float3((**other).position()), make_float3(v.position()));            const float distanceSquared = float3_dot(offset, offset);			steering = float3_add(steering, float3_scalar_divide(offset, -distanceSquared));            // count neighbors            neighbors++;        }    }    // divide by neighbors, then normalize to pure direction    if (neighbors > 0) 		steering = float3_normalize(float3_scalar_divide(steering, (float)neighbors));    return steering;}

float3OpenSteer::SteerLibrary::steerForCohesion (const AbstractVehicle& v, 				  const float maxDistance,                  const float cosMaxAngle,                  const AVGroup& flock){    // steering accumulator and count of neighbors, both initially zero    float3 steering = float3_zero();    int neighbors = 0;    // for each of the other vehicles...    for (AVIterator other = flock.begin(); other != flock.end(); other++)    {        if (inBoidNeighborhood (v, **other, v.radius() * 3, maxDistance, cosMaxAngle))        {            // accumulate sum of neighbor's positions			steering = float3_add(steering, make_float3((**other).position()));            // count neighbors            neighbors++;        }    }    // divide by neighbors, subtract off current position to get error-    // correcting direction, then normalize to pure direction    if (neighbors > 0)		steering = float3_normalize(float3_subtract(float3_scalar_divide(steering, (float)neighbors), make_float3(v.position())));    return steering;}

float3OpenSteer::SteerLibrary::steerToStayOnPath (const AbstractVehicle& v, 				   const float predictionTime, 				   Pathway& path){    // predict our future position    const float3 futurePosition = v.predictFuturePosition (predictionTime);    // find the point on the path nearest the predicted future position    float3 tangent;    float outside;    const float3 onPath = path.mapPointToPath (futurePosition,                                             tangent,     // output argument                                             outside);    // output argument    if (outside < 0)    {        // our predicted future position was in the path,        // return zero steering.        return float3_zero();    }    else    {        // our predicted future position was outside the path, need to        // steer towards it.  Use onPath projection of futurePosition        // as seek target        annotatePathFollowing (futurePosition, onPath, onPath, outside);        return steerForSeek (v, onPath);    }}

void OpenSteer::PlaneObstacle::findIntersectionWithVehiclePath (const AbstractVehicle& vehicle,                                 PathIntersection& pi) const{    // initialize pathIntersection object to "no intersection found"    pi.intersect = false;    const Vec3 lp =  localizePosition (vehicle.position ());    const Vec3 ld = localizeDirection (vehicle.forward ());    // no obstacle intersection if path is parallel to XY (side/up) plane    if (ld.dot (Vec3::forward) == 0.0f) return;    // no obstacle intersection if vehicle is heading away from the XY plane    if ((lp.z > 0.0f) && (ld.z > 0.0f)) return;    if ((lp.z < 0.0f) && (ld.z < 0.0f)) return;    // no obstacle intersection if obstacle "not seen" from vehicle's side    if ((seenFrom () == outside) && (lp.z < 0.0f)) return;    if ((seenFrom () == inside)  && (lp.z > 0.0f)) return;    // find intersection of path with rectangle's plane (XY plane)    const float ix = lp.x - (ld.x * lp.z / ld.z);    const float iy = lp.y - (ld.y * lp.z / ld.z);    const Vec3 planeIntersection (ix, iy, 0.0f);    // no obstacle intersection if plane intersection is outside 2d shape    if (!xyPointInsideShape (planeIntersection, vehicle.radius ())) return;    // otherwise, the vehicle path DOES intersect this rectangle    const Vec3 localXYradial = planeIntersection.normalize ();    const Vec3 radial = globalizeDirection (localXYradial);    const float sideSign = (lp.z > 0.0f) ? +1.0f : -1.0f;    const Vec3 opposingNormal = forward () * sideSign;    pi.intersect = true;    pi.obstacle = this;    pi.distance = (lp - planeIntersection).length ();    pi.steerHint = opposingNormal + radial; // should have "toward edge" term?    pi.surfacePoint = globalizePosition (planeIntersection);    pi.surfaceNormal = opposingNormal;    pi.vehicleOutside = lp.z > 0.0f;}

float3OpenSteer::SteerLibrary::steerForEvasion (const AbstractVehicle& v, 				 const AbstractVehicle& menace,                 const float maxPredictionTime){    // offset from this to menace, that distance, unit vector toward menace    const float3 offset = float3_subtract(make_float3(menace.position()), make_float3(v.position()));    const float distance = float3_length(offset);    const float roughTime = distance / menace.speed();    const float predictionTime = ((roughTime > maxPredictionTime) ?                                  maxPredictionTime :                                  roughTime);    const float3 target = menace.predictFuturePosition (predictionTime);    return steerForFlee (v, target);}

void OpenSteer::OpenSteerDemo::drawBoxHighlightOnVehicle (const AbstractVehicle& v,                                               const Color& color){    if (&v)    {        const float diameter = v.radius() * 2;        const Vec3 size (diameter, diameter, diameter);        drawBoxOutline (v, size, color);    }}

float3OpenSteer::SteerLibrary::steerForWander (const AbstractVehicle& v, float dt){    // random walk WanderSide and WanderUp between -1 and +1    const float speed = 12 * dt; // maybe this (12) should be an argument?    WanderSide = scalarRandomWalk (WanderSide, speed, -1, +1);    WanderUp   = scalarRandomWalk (WanderUp,   speed, -1, +1);    // return a pure lateral steering vector: (+/-Side) + (+/-Up)    return float3_add(float3_scalar_multiply(v.side(), WanderSide), float3_scalar_multiply(v.up(), WanderUp));}

OpenSteer::Vec3 OpenSteer::Obstacle::PathIntersection::steerToAvoidIfNeeded (const AbstractVehicle& vehicle,                      const float minTimeToCollision) const{    // if nearby intersection found, steer away from it, otherwise no steering    const float minDistanceToCollision = minTimeToCollision * vehicle.speed();    if (intersect && (distance < minDistanceToCollision))    {        // compute avoidance steering force: take the component of        // steerHint which is lateral (perpendicular to vehicle's        // forward direction), set its length to vehicle's maxForce        Vec3 lateral = steerHint.perpendicularComponent (vehicle.forward ());        if (lateral == Vec3::zero)            lateral = vehicle.side ();        return lateral.normalize () * vehicle.maxForce ();    }    else    {        return Vec3::zero;    }}

	//-----------------------------------------------------------------------------	// (parameter names commented out to prevent compiler warning from "-W")	void Pedestrian::annotateAvoidNeighbor (const AbstractVehicle& threat,		const float /*steer*/,		const osVector3& ourFuture,		const osVector3& threatFuture)	{		const Color green (0.15f, 0.6f, 0.0f);		annotationLine( position(), ourFuture, green );		annotationLine( threat.position(), threatFuture, green );		annotationLine( ourFuture, threatFuture, gRed );		annotationXZCircle( radius(), ourFuture,    green, 12 );		annotationXZCircle( radius(), threatFuture, green, 12 );	}

	//-----------------------------------------------------------------------------	// called when steerToAvoidCloseNeighbors decides steering is required	// (parameter names commented out to prevent compiler warning from "-W")	void Pedestrian::annotateAvoidCloseNeighbor (const AbstractVehicle& other,		const float /*additionalDistance*/)	{		// draw the word "Ouch!" above colliding vehicles		const float headOn = forward().dot(other.forward()) < 0;		const Color green (0.4f, 0.8f, 0.1f);		const Color red (1, 0.1f, 0);		const Color color = headOn ? red : green;		const char* string = headOn ? "OUCH!" : "pardon me";		const osVector3 location = position() + osVector3 (0, 0.5f, 0);		if (annotationIsOn())			draw2dTextAt3dLocation (string, location, color, drawGetWindowWidth(), drawGetWindowHeight());	}

boolOpenSteer::SteerLibrary::inBoidNeighborhood (const AbstractVehicle& v, 					const AbstractVehicle& other,                    const float minDistance,                    const float maxDistance,                    const float cosMaxAngle){    if (&other == &v)    {        return false;    }    else    {        const float3 offset = float3_subtract(make_float3(other.position()), make_float3(v.position()));		const float distanceSquared = float3_lengthSquared(offset);        // definitely in neighborhood if inside minDistance sphere        if (distanceSquared < (minDistance * minDistance))        {            return true;        }        else        {            // definitely not in neighborhood if outside maxDistance sphere            if (distanceSquared > (maxDistance * maxDistance))            {                return false;            }            else            {                // otherwise, test angular offset from forward axis				const float3 unitOffset = float3_scalar_divide(offset, sqrt(distanceSquared));                const float forwardness = float3_dot(make_float3(v.forward()), unitOffset);                return forwardness > cosMaxAngle;            }        }    }}

float3OpenSteer::SteerLibrary::steerToAvoidObstacle (const AbstractVehicle& v, 					  const float minTimeToCollision,                      const Obstacle& obstacle){    const float3 avoidance = obstacle.steerToAvoid (v, minTimeToCollision);    // XXX more annotation modularity problems (assumes spherical obstacle)    if (!float3_equals(avoidance, float3_zero()))        annotateAvoidObstacle (minTimeToCollision * v.speed());    return avoidance;}

floatOpenSteer::SteerLibrary::computeNearestApproachPositions (const AbstractVehicle& v, 								 AbstractVehicle& other, 								 float time){	const float3 myTravel = float3_scalar_multiply(make_float3(v.forward()), v.speed () * time);	const float3 otherTravel = float3_scalar_multiply(make_float3(other.forward()), other.speed () * time);    const float3 myFinal = float3_add(make_float3(v.position()), myTravel);    const float3 otherFinal = float3_add(make_float3(other.position()), otherTravel);    // xxx for annotation    ourPositionAtNearestApproach = myFinal;    hisPositionAtNearestApproach = otherFinal;	return float3_distance(myFinal, otherFinal);}

void OpenSteer::OpenSteerDemo::highlightVehicleUtility (const AbstractVehicle& vehicle){    if (&vehicle != NULL)        drawXZDisk (vehicle.radius(), vehicle.position(), gGray60, 20);}

float3OpenSteer::SteerLibrary::steerToAvoidNeighbors (const AbstractVehicle& v, 					   const float minTimeToCollision,                       const AVGroup& others){    // first priority is to prevent immediate interpenetration    const float3 separation = steerToAvoidCloseNeighbors (v, 0, others);    	if (!float3_equals(separation, float3_zero()))		return separation;    // otherwise, go on to consider potential future collisions    float steer = 0;    AbstractVehicle* threat = NULL;    // Time (in seconds) until the most immediate collision threat found    // so far.  Initial value is a threshold: don't look more than this    // many frames into the future.    float minTime = minTimeToCollision;    // xxx solely for annotation    float3 xxxThreatPositionAtNearestApproach;    float3 xxxOurPositionAtNearestApproach;    // for each of the other vehicles, determine which (if any)    // pose the most immediate threat of collision.    for (AVIterator i = others.begin(); i != others.end(); i++)    {        AbstractVehicle& other = **i;        if (&other != &v)        {	            // avoid when future positions are this close (or less)            const float collisionDangerThreshold = v.radius() * 2;            // predicted time until nearest approach of "this" and "other"            const float time = predictNearestApproachTime (v, other);            // If the time is in the future, sooner than any other            // threatened collision...            if ((time >= 0) && (time < minTime))            {                // if the two will be close enough to collide,                // make a note of it                if (computeNearestApproachPositions (v, other, time)                    < collisionDangerThreshold)                {                    minTime = time;                    threat = &other;                    xxxThreatPositionAtNearestApproach                        = hisPositionAtNearestApproach;                    xxxOurPositionAtNearestApproach                        = ourPositionAtNearestApproach;                }            }        }    }    // if a potential collision was found, compute steering to avoid    if (threat != NULL)    {        // parallel: +1, perpendicular: 0, anti-parallel: -1        float parallelness = float3_dot(make_float3(v.forward()), make_float3(threat->forward()));        float angle = 0.707f;        if (parallelness < -angle)        {            // anti-parallel "head on" paths:            // steer away from future threat position            float3 offset = float3_subtract(xxxThreatPositionAtNearestApproach, make_float3(v.position()));            float sideDot = float3_dot(offset, v.side());            steer = (sideDot > 0) ? -1.0f : 1.0f;        }        else        {            if (parallelness > angle)            {                // parallel paths: steer away from threat                float3 offset = float3_subtract(make_float3(threat->position()), make_float3(v.position()));                float sideDot = float3_dot(offset, v.side());                steer = (sideDot > 0) ? -1.0f : 1.0f;            }            else            {                // perpendicular paths: steer behind threat                // (only the slower of the two does this)                if (threat->speed() <= v.speed())                {                    float sideDot = float3_dot(v.side(), threat->velocity());                    steer = (sideDot > 0) ? -1.0f : 1.0f;                }            }        }        annotateAvoidNeighbor (*threat,                               steer,                               xxxOurPositionAtNearestApproach,                               xxxThreatPositionAtNearestApproach);    }//.........这里部分代码省略.........

float3OpenSteer::SteerLibrary::steerForPursuit (const AbstractVehicle& v, 				 const AbstractVehicle& quarry,                 const float maxPredictionTime){    // offset from this to quarry, that distance, unit vector toward quarry    const float3 offset = float3_subtract(make_float3(quarry.position()), make_float3(v.position()));	const float distance = float3_length(offset);    const float3 unitOffset = float3_scalar_divide(offset, distance);    // how parallel are the paths of "this" and the quarry    // (1 means parallel, 0 is pependicular, -1 is anti-parallel)    const float parallelness = float3_dot(make_float3(v.forward()), make_float3(quarry.forward()));    // how "forward" is the direction to the quarry    // (1 means dead ahead, 0 is directly to the side, -1 is straight back)    const float forwardness = float3_dot(make_float3(v.forward()), unitOffset);    const float directTravelTime = distance / v.speed ();    const int f = intervalComparison (forwardness,  -0.707f, 0.707f);    const int p = intervalComparison (parallelness, -0.707f, 0.707f);    float timeFactor = 0; // to be filled in below    float3 color;           // to be filled in below (xxx just for debugging)    // Break the pursuit into nine cases, the cross product of the    // quarry being [ahead, aside, or behind] us and heading    // [parallel, perpendicular, or anti-parallel] to us.    switch (f)    {    case +1:        switch (p)        {        case +1:          // ahead, parallel            timeFactor = 4;            color = gBlack;            break;        case 0:           // ahead, perpendicular            timeFactor = 1.8f;            color = gGray50;            break;        case -1:          // ahead, anti-parallel            timeFactor = 0.85f;            color = gWhite;            break;        }        break;    case 0:        switch (p)        {        case +1:          // aside, parallel            timeFactor = 1;            color = gRed;            break;        case 0:           // aside, perpendicular            timeFactor = 0.8f;            color = gYellow;            break;        case -1:          // aside, anti-parallel            timeFactor = 4;            color = gGreen;            break;        }        break;    case -1:        switch (p)        {        case +1:          // behind, parallel            timeFactor = 0.5f;            color= gCyan;            break;        case 0:           // behind, perpendicular            timeFactor = 2;            color= gBlue;            break;        case -1:          // behind, anti-parallel            timeFactor = 2;            color = gMagenta;            break;        }        break;    }    // estimated time until intercept of quarry    const float et = directTravelTime * timeFactor;    // xxx experiment, if kept, this limit should be an argument    const float etl = (et > maxPredictionTime) ? maxPredictionTime : et;    // estimated position of quarry at intercept    const float3 target = quarry.predictFuturePosition (etl);    // annotation    annotationLine (make_float3(v.position()),                    target,                    gaudyPursuitAnnotation ? color : gGray40);    return steerForSeek (v, target);}