C++ AbstractVehicle::localizePosition方法代码示例

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::
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;
    }
}