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; }
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:: 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(); }
void OpenSteer::OpenSteerDemo::circleHighlightVehicleUtility (const AbstractVehicle& vehicle) { if (&vehicle != NULL) drawXZCircle (vehicle.radius () * 1.1f, vehicle.position(), gGray60, 20); }
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); } }
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; }
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::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:: 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; }
void OpenSteer::OpenSteerDemo::highlightVehicleUtility (const AbstractVehicle& vehicle) { if (&vehicle != NULL) drawXZDisk (vehicle.radius(), vehicle.position(), gGray60, 20); }
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:: 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; } }