Пример #1
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);
    }
}
Пример #2
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);
    }
}
Пример #3
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);
}
Vector3 SteeringVehicle::calcSeek(const Vector3& target)
{
	Vector3 steering = Vec3Utils::setYtoZero(steerForSeek(target));
	return steering;
}