Exemplo n.º 1
0
//-----------------------------------------------------------------------------
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);
}
Exemplo n.º 2
0
float3
OpenSteer::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();
}
Exemplo n.º 3
0
float3
OpenSteer::SteerLibrary::
steerForFlee (const AbstractVehicle& v, const float3& target)
{
    const float3 desiredVelocity = float3_subtract(make_float3(v.position()), target);
    return float3_subtract(desiredVelocity, v.velocity());
}
Exemplo n.º 4
0
float
OpenSteer::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;
}
Exemplo n.º 5
0
float3
OpenSteer::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);
}
Exemplo n.º 7
0
float3
OpenSteer::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));
}
Exemplo n.º 8
0
float3
OpenSteer::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);
    }
}
Exemplo n.º 9
0
void
OpenSteer::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;
}
Exemplo n.º 10
0
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*/)
{
    selectedVehicle = &selected;
    if (&selected)
    {
        const Vec3 behind = selected.forward() * -distance;
        camera.setPosition (selected.position() + behind);
        camera.target = 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();
	}
}
Exemplo n.º 13
0
void 
OpenSteer::OpenSteerDemo::drawCircleHighlightOnVehicle (const AbstractVehicle& v,
                                                  const float radiusMultiplier,
                                                  const float3 color)
{
    if (&v)
    {
        const float3& cPosition = make_float3(camera.position());
        draw3dCircle  (v.radius() * radiusMultiplier,  // adjusted radius
                       make_float3(v.position()),                   // center
                       float3_subtract(make_float3(v.position()), cPosition),       // view axis
                       color,                          // drawing color
                       20);                            // circle segments
    }
}
void 
OpenSteer::OpenSteerDemo::drawCircleHighlightOnVehicle (const AbstractVehicle& v,
                                                  const float radiusMultiplier,
                                                  const Color& color)
{
    if (&v)
    {
        const Vec3& cPosition = camera.position();
        draw3dCircle  (v.radius() * radiusMultiplier,  // adjusted radius
                       v.position(),                   // center
                       v.position() - cPosition,       // view axis
                       color,                          // drawing color
                       20);                            // circle segments
    }
}
Exemplo n.º 15
0
float3
OpenSteer::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;
}
Exemplo n.º 16
0
float3
OpenSteer::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;
}
Exemplo n.º 17
0
float3
OpenSteer::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);
    }
}
Exemplo n.º 18
0
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;
}
Exemplo n.º 19
0
float3
OpenSteer::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);
    }
}
Exemplo n.º 21
0
float3
OpenSteer::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));
}
Exemplo n.º 22
0
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());
	}
Exemplo n.º 25
0
bool
OpenSteer::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;
            }
        }
    }
}
Exemplo n.º 26
0
float3
OpenSteer::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;
}
Exemplo n.º 27
0
float
OpenSteer::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);
}
Exemplo n.º 29
0
float3
OpenSteer::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);
    }

	return float3_scalar_multiply(v.side(), steer);
}
Exemplo n.º 30
0
float3
OpenSteer::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);
}