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();
}
}
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
}
}
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());
}
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;
}
//-----------------------------------------------------------------------------
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);
}
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);
}
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);
}
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);
}
}
//-----------------------------------------------------------------------------
// (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 );
}
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
}
}
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;
}
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);
}
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;
}
}
}
}
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;
}
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);
}
void
OpenSteer::OpenSteerDemo::highlightVehicleUtility (const AbstractVehicle& vehicle)
{
if (&vehicle != NULL)
drawXZDisk (vehicle.radius(), vehicle.position(), gGray60, 20);
}
void
OpenSteer::
SphereObstacle::
findIntersectionWithVehiclePath (const AbstractVehicle& vehicle,
PathIntersection& pi) const
{
// 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/
// But the computation is done in the vehicle's local space, so
// the line in question is the Z (Forward) axis of the space which
// simplifies some of the calculations.
float b, c, d, p, q, s;
Vec3 lc;
// initialize pathIntersection object to "no intersection found"
pi.intersect = false;
// find sphere's "local center" (lc) in the vehicle's coordinate space
lc = vehicle.localizePosition (center);
pi.vehicleOutside = lc.length () > radius;
// if obstacle is seen from inside, but vehicle is outside, must avoid
// (noticed once a vehicle got outside it ignored the obstacle 2008-5-20)
if (pi.vehicleOutside && (seenFrom () == inside))
{
pi.intersect = true;
pi.distance = 0.0f;
pi.steerHint = (center - vehicle.position()).normalize();
return;
}
// compute line-sphere intersection parameters
const float r = radius + vehicle.radius();
b = -2 * lc.z;
c = square (lc.x) + square (lc.y) + square (lc.z) - square (r);
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, so intersects our forward
// path
pi.intersect = true;
pi.obstacle = this;
pi.distance =
((p > 0) && (q > 0)) ?
// both intersections are in front of us, find nearest one
((p < q) ? p : q) :
// otherwise one is ahead and one is behind: we are INSIDE obstacle
(seenFrom () == outside ?
// inside a solid obstacle, so distance to obstacle is zero
0.0f :
// hollow obstacle (or "both"), pick point that is in front
((p > 0) ? p : q));
pi.surfacePoint =
vehicle.position() + (vehicle.forward() * pi.distance);
pi.surfaceNormal = (pi.surfacePoint-center).normalize();
switch (seenFrom ())
{
case outside:
pi.steerHint = pi.surfaceNormal;
break;
case inside:
pi.steerHint = -pi.surfaceNormal;
break;
case both:
pi.steerHint = pi.surfaceNormal * (pi.vehicleOutside ? 1.0f : -1.0f);
break;
}
}
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();
}
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);
}
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;
}