515 lines
17 KiB
C++
515 lines
17 KiB
C++
// ----------------------------------------------------------------------------
|
|
//
|
|
//
|
|
// OpenSteer -- Steering Behaviors for Autonomous Characters
|
|
//
|
|
// Copyright (c) 2002-2005, Sony Computer Entertainment America
|
|
// Original author: Craig Reynolds <craig_reynolds@playstation.sony.com>
|
|
//
|
|
// Permission is hereby granted, free of charge, to any person obtaining a
|
|
// copy of this software and associated documentation files (the "Software"),
|
|
// to deal in the Software without restriction, including without limitation
|
|
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
|
|
// and/or sell copies of the Software, and to permit persons to whom the
|
|
// Software is furnished to do so, subject to the following conditions:
|
|
//
|
|
// The above copyright notice and this permission notice shall be included in
|
|
// all copies or substantial portions of the Software.
|
|
//
|
|
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
|
|
// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
|
|
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
|
|
// DEALINGS IN THE SOFTWARE.
|
|
//
|
|
//
|
|
// ----------------------------------------------------------------------------
|
|
//
|
|
//
|
|
// Utilities for OpenSteering
|
|
//
|
|
// 08-06-05 bk: added functions to clamp values to a certain value range, to
|
|
// compare values using a tolerance, and so on.
|
|
// 10-04-04 bk: put everything into the OpenSteer namespace
|
|
// 07-09-02 cwr: created
|
|
//
|
|
//
|
|
// ----------------------------------------------------------------------------
|
|
|
|
|
|
#ifndef OPENSTEER_UTILITIES_H
|
|
#define OPENSTEER_UTILITIES_H
|
|
|
|
|
|
#include <iostream> // for ostream, <<, etc.
|
|
#include <cstdlib> // for rand, etc.
|
|
#include <cfloat> // for FLT_MAX, etc.
|
|
#include <cmath> // for sqrt, etc.
|
|
#include <vector> // for std::vector
|
|
#include <cassert> // for assert
|
|
#include <limits> // for numeric_limits
|
|
|
|
// ----------------------------------------------------------------------------
|
|
// For the sake of Windows, apparently this is a "Linux/Unix thing"
|
|
|
|
|
|
#ifndef OPENSTEER_M_PI
|
|
#define OPENSTEER_M_PI 3.14159265358979323846f
|
|
#endif
|
|
|
|
#ifdef _MSC_VER
|
|
#undef min
|
|
#undef max
|
|
#endif
|
|
|
|
|
|
namespace OpenSteer {
|
|
|
|
// ----------------------------------------------------------------------------
|
|
// Generic interpolation
|
|
|
|
|
|
template<class T> inline T interpolate (float alpha, const T& x0, const T& x1)
|
|
{
|
|
return x0 + ((x1 - x0) * alpha);
|
|
}
|
|
|
|
|
|
// ----------------------------------------------------------------------------
|
|
// Random number utilities
|
|
|
|
|
|
// Returns a float randomly distributed between 0 and 1
|
|
|
|
inline float frandom01 (void)
|
|
{
|
|
return (((float) rand ()) / ((float) RAND_MAX));
|
|
}
|
|
|
|
|
|
// Returns a float randomly distributed between lowerBound and upperBound
|
|
|
|
inline float frandom2 (float lowerBound, float upperBound)
|
|
{
|
|
return lowerBound + (frandom01 () * (upperBound - lowerBound));
|
|
}
|
|
|
|
|
|
// ----------------------------------------------------------------------------
|
|
// Constrain a given value (x) to be between two (ordered) bounds: min
|
|
// and max. Returns x if it is between the bounds, otherwise returns
|
|
// the nearer bound.
|
|
|
|
|
|
inline float clip (const float x, const float min, const float max)
|
|
{
|
|
if (x < min) return min;
|
|
if (x > max) return max;
|
|
return x;
|
|
}
|
|
|
|
|
|
// ----------------------------------------------------------------------------
|
|
// remap a value specified relative to a pair of bounding values
|
|
// to the corresponding value relative to another pair of bounds.
|
|
// Inspired by (dyna:remap-interval y y0 y1 z0 z1)
|
|
|
|
|
|
inline float remapInterval (float x,
|
|
float in0, float in1,
|
|
float out0, float out1)
|
|
{
|
|
// uninterpolate: what is x relative to the interval in0:in1?
|
|
float relative = (x - in0) / (in1 - in0);
|
|
|
|
// now interpolate between output interval based on relative x
|
|
return interpolate (relative, out0, out1);
|
|
}
|
|
|
|
|
|
// Like remapInterval but the result is clipped to remain between
|
|
// out0 and out1
|
|
|
|
|
|
inline float remapIntervalClip (float x,
|
|
float in0, float in1,
|
|
float out0, float out1)
|
|
{
|
|
// uninterpolate: what is x relative to the interval in0:in1?
|
|
float relative = (x - in0) / (in1 - in0);
|
|
|
|
// now interpolate between output interval based on relative x
|
|
return interpolate (clip (relative, 0, 1), out0, out1);
|
|
}
|
|
|
|
|
|
// ----------------------------------------------------------------------------
|
|
// classify a value relative to the interval between two bounds:
|
|
// returns -1 when below the lower bound
|
|
// returns 0 when between the bounds (inside the interval)
|
|
// returns +1 when above the upper bound
|
|
|
|
|
|
inline int intervalComparison (float x, float lowerBound, float upperBound)
|
|
{
|
|
if (x < lowerBound) return -1;
|
|
if (x > upperBound) return +1;
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
// ----------------------------------------------------------------------------
|
|
|
|
|
|
inline float scalarRandomWalk (const float initial,
|
|
const float walkspeed,
|
|
const float min,
|
|
const float max)
|
|
{
|
|
const float next = initial + (((frandom01() * 2) - 1) * walkspeed);
|
|
if (next < min) return min;
|
|
if (next > max) return max;
|
|
return next;
|
|
}
|
|
|
|
|
|
// ----------------------------------------------------------------------------
|
|
|
|
|
|
inline float square (float x)
|
|
{
|
|
return x * x;
|
|
}
|
|
|
|
|
|
// ----------------------------------------------------------------------------
|
|
// for debugging: prints one line with a given C expression, an equals sign,
|
|
// and the value of the expression. For example "angle = 35.6"
|
|
|
|
|
|
#define debugPrint(e) (std::cout << #e" = " << (e) << std::endl << std::flush)
|
|
|
|
|
|
// ----------------------------------------------------------------------------
|
|
// blends new values into an accumulator to produce a smoothed time series
|
|
//
|
|
// Modifies its third argument, a reference to the float accumulator holding
|
|
// the "smoothed time series."
|
|
//
|
|
// The first argument (smoothRate) is typically made proportional to "dt" the
|
|
// simulation time step. If smoothRate is 0 the accumulator will not change,
|
|
// if smoothRate is 1 the accumulator will be set to the new value with no
|
|
// smoothing. Useful values are "near zero".
|
|
//
|
|
// Usage:
|
|
// blendIntoAccumulator (dt * 0.4f, currentFPS, smoothedFPS);
|
|
|
|
|
|
template<class T>
|
|
inline void blendIntoAccumulator (const float smoothRate,
|
|
const T& newValue,
|
|
T& smoothedAccumulator)
|
|
{
|
|
smoothedAccumulator = interpolate (clip (smoothRate, 0, 1),
|
|
smoothedAccumulator,
|
|
newValue);
|
|
}
|
|
|
|
|
|
// ----------------------------------------------------------------------------
|
|
// given a new Angle and an old angle, adjust the new for angle wraparound (the
|
|
// 0->360 flip), returning a value equivalent to newAngle, but closest in
|
|
// absolute value to oldAngle. For radians fullCircle = OPENSTEER_M_PI*2, for degrees
|
|
// fullCircle = 360. Based on code in stuart/bird/fish demo's camera.cc
|
|
//
|
|
// (not currently used)
|
|
|
|
/*
|
|
inline float distance1D (const float a, const float b)
|
|
{
|
|
const float d = a - b;
|
|
return (d > 0) ? d : -d;
|
|
}
|
|
|
|
|
|
float adjustForAngleWraparound (float newAngle,
|
|
float oldAngle,
|
|
float fullCircle)
|
|
{
|
|
// adjust newAngle for angle wraparound: consider its current value (a)
|
|
// as well as the angle 2pi larger (b) and 2pi smaller (c). Select the
|
|
// one closer (magnitude of difference) to the current value of oldAngle.
|
|
const float a = newAngle;
|
|
const float b = newAngle + fullCircle;
|
|
const float c = newAngle - fullCircle;
|
|
const float ad = distance1D (a, oldAngle);
|
|
const float bd = distance1D (b, oldAngle);
|
|
const float cd = distance1D (c, oldAngle);
|
|
|
|
if ((bd < ad) && (bd < cd)) return b;
|
|
if ((cd < ad) && (cd < bd)) return c;
|
|
return a;
|
|
}
|
|
*/
|
|
|
|
|
|
// ----------------------------------------------------------------------------
|
|
// Functions to encapsulate cross-platform differences for several <cmath>
|
|
// functions. Specifically, the C++ standard says that these functions are
|
|
// in the std namespace (std::sqrt, etc.) Apparently the MS VC6 compiler (or
|
|
// its header files) do not implement this correctly and the function names
|
|
// are in the global namespace. We hope these -XXX versions are a temporary
|
|
// expedient, to be removed later.
|
|
|
|
|
|
#ifdef _WIN32
|
|
|
|
inline float floorXXX (float x) {return ::floor (x);}
|
|
inline float sqrtXXX (float x) {return ::sqrt (x);}
|
|
inline float sinXXX (float x) {return ::sin (x);}
|
|
inline float cosXXX (float x) {return ::cos (x);}
|
|
inline float absXXX (float x) {return ::abs (x);}
|
|
inline int absXXX (int x) {return ::abs (x);}
|
|
inline float maxXXX (float x, float y) {if (x > y) return x; else return y;}
|
|
inline float minXXX (float x, float y) {if (x < y) return x; else return y;}
|
|
|
|
#else
|
|
|
|
inline float floorXXX (float x) {return std::floor (x);}
|
|
inline float sqrtXXX (float x) {return std::sqrt (x);}
|
|
inline float sinXXX (float x) {return std::sin (x);}
|
|
inline float cosXXX (float x) {return std::cos (x);}
|
|
inline float absXXX (float x) {return std::abs (x);}
|
|
inline int absXXX (int x) {return std::abs (x);}
|
|
inline float maxXXX (float x, float y) {return std::max (x, y);}
|
|
inline float minXXX (float x, float y) {return std::min (x, y);}
|
|
|
|
#endif
|
|
|
|
|
|
// ----------------------------------------------------------------------------
|
|
// round (x) "round off" x to the nearest integer (as a float value)
|
|
//
|
|
// This is a Gnu-sanctioned(?) post-ANSI-Standard(?) extension (as in
|
|
// http://www.opengroup.org/onlinepubs/007904975/basedefs/math.h.html)
|
|
// which may not be present in all C++ environments. It is defined in
|
|
// math.h headers in Linux and Mac OS X, but apparently not in Win32:
|
|
|
|
|
|
#ifdef _WIN32
|
|
|
|
inline float round (float x)
|
|
{
|
|
if (x < 0)
|
|
return -floorXXX (0.5f - x);
|
|
else
|
|
return floorXXX (0.5f + x);
|
|
}
|
|
|
|
#else
|
|
|
|
inline float round( float x )
|
|
{
|
|
return ::round( x );
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
/**
|
|
* Returns @a valueToClamp clamped to the range @a minValue - @a maxValue.
|
|
*/
|
|
template< typename T >
|
|
T
|
|
clamp( T const& valueToClamp, T const& minValue, T const& maxValue) {
|
|
assert( minValue <= maxValue && "minValue must be lesser or equal to maxValue." );
|
|
|
|
if ( valueToClamp < minValue ) {
|
|
return minValue;
|
|
} else if ( valueToClamp > maxValue ) {
|
|
return maxValue;
|
|
}
|
|
|
|
return valueToClamp;
|
|
}
|
|
|
|
|
|
/**
|
|
* Returns the floating point remainder of the division of @a x by @a y.
|
|
* If @a y is @c 0 the behavior is undefined.
|
|
*/
|
|
inline float modulo( float x, float y ) {
|
|
assert( 0.0f != y && "Division by zero." );
|
|
return std::fmod( x, y );
|
|
}
|
|
|
|
/**
|
|
* Returns the floating point remainder of the division of @a x by @a y.
|
|
* If @a y is @c 0 the behavior is undefined.
|
|
*/
|
|
inline double modulo( double x, double y ) {
|
|
assert( 0.0 != y && "Division by zero." );
|
|
return std::fmod( x, y );
|
|
}
|
|
|
|
/**
|
|
* Returns the floating point remainder of the division of @a x by @a y.
|
|
* If @a y is @c 0 the behavior is undefined.
|
|
*/
|
|
inline long double modulo( long double x, long double y ) {
|
|
assert( 0.0 != y && "Division by zero." );
|
|
return std::fmod( x, y );
|
|
}
|
|
|
|
/**
|
|
* Returns the floating point remainder of the division of @a x by @a y.
|
|
* If @a y is @c 0 the behavior is undefined.
|
|
*/
|
|
inline short modulo( short x, short y ) {
|
|
assert( 0 != y && "Division by zero." );
|
|
return x % y;
|
|
}
|
|
|
|
/**
|
|
* Returns the floating point remainder of the division of @a x by @a y.
|
|
* If @a y is @c 0 the behavior is undefined.
|
|
*/
|
|
inline int modulo( int x, int y ) {
|
|
assert( 0 != y && "Division by zero." );
|
|
return x % y;
|
|
}
|
|
|
|
/**
|
|
* Returns the floating point remainder of the division of @a x by @a y.
|
|
* If @a y is @c 0 the behavior is undefined.
|
|
*/
|
|
inline long modulo( long x, long y ) {
|
|
assert( 0 != y && "Division by zero." );
|
|
return x % y;
|
|
}
|
|
|
|
|
|
/**
|
|
* Returns <code>value</code> if <code>value >= 0 </code>, otherwise
|
|
* <code>-value</code>.
|
|
*/
|
|
template< typename T >
|
|
T abs( T const& value ) {
|
|
return absXXX( value );
|
|
}
|
|
|
|
/**
|
|
* Returns the maximum of the three values @a v0, @a v1, and @a v2.
|
|
*
|
|
* @todo Write a unit test.
|
|
*/
|
|
template< typename T >
|
|
T
|
|
max( T const& v0, T const& v1, T const& v2 ) {
|
|
return maxXXX( v0, maxXXX( v1, v2 ) );
|
|
}
|
|
|
|
|
|
/**
|
|
* Returns the minimum of the three values @a v0, @a v1, and @a v2.
|
|
*
|
|
* @todo Write a unit test.
|
|
*/
|
|
template< typename T >
|
|
T
|
|
min( T const& v0, T const& v1, T const& v2 ) {
|
|
return minXXX( v0, minXXX( v1, v2 ) );
|
|
}
|
|
|
|
|
|
/**
|
|
* Compares the absolute value of @a v with @a tolerance.
|
|
*
|
|
* See Christer Ericson, Real-Time Collision Detection, Morgan Kaufmann,
|
|
* 2005, pp. 441--443.
|
|
*
|
|
* @todo Write a unit test.
|
|
*/
|
|
template< typename T >
|
|
bool
|
|
isZero( T const& v, T const& tolerance = std::numeric_limits< T >::epsilon() ) {
|
|
return abs( v ) <= tolerance;
|
|
}
|
|
|
|
|
|
/**
|
|
* Compares @a lhs with @a rhs given a specific @a tolerance.
|
|
*
|
|
* @attention Adapt @a tolerance to the range of values of @a lhs and
|
|
* @a rhs.
|
|
* See Christer Ericson, Real-Time Collision Detection, Morgan Kaufmann,
|
|
* 2005, pp. 441--443.
|
|
*
|
|
* @return <code>abs( lhs - rhs ) <= tolerance</code>
|
|
*
|
|
* @todo Write a unit test.
|
|
*/
|
|
template< typename T >
|
|
bool
|
|
equalsAbsolute( T const& lhs, T const& rhs, T const& tolerance = std::numeric_limits< T >::epsilon() ) {
|
|
return isZero( lhs - rhs, tolerance );
|
|
}
|
|
|
|
|
|
/**
|
|
* Compares @a lhs with @a rhs given a specific @a tolerance taking the
|
|
* range of values into account.
|
|
*
|
|
* See Christer Ericson, Real-Time Collision Detection, Morgan Kaufmann,
|
|
* 2005, pp. 441--443.
|
|
*
|
|
* @return <code>abs( lhs - rhs ) <= tolerance * max( abs( lhs ), abs( rhs ), 1 )</code>
|
|
*
|
|
* @todo Write a unit test.
|
|
*/
|
|
template< typename T >
|
|
bool
|
|
equalsRelative( T const& lhs, T const& rhs, T const& tolerance = std::numeric_limits< T >::epsilon() ) {
|
|
return isZero( lhs - rhs, tolerance * max( abs( lhs ), abs( rhs ), T( 1 ) ) );
|
|
}
|
|
|
|
|
|
/**
|
|
* Approximately compares @a lhs with @a rhs given a specific @a tolerance
|
|
* taking the range of values into account.
|
|
*
|
|
* See Christer Ericson, Real-Time Collision Detection, Morgan Kaufmann,
|
|
* 2005, pp. 441--443.
|
|
*
|
|
* @return <code>abs( lhs - rhs ) <= tolerance * ( abs( lhs ) + abs( rhs ) + 1 )</code>
|
|
*
|
|
* @todo Write a unit test.
|
|
*/
|
|
template< typename T >
|
|
bool
|
|
equalsRelativeApproximately( T const& lhs, T const& rhs, T const& tolerance = std::numeric_limits< T >::epsilon() ) {
|
|
return isZero( lhs - rhs, tolerance * ( abs( lhs ) + abs( rhs ) + T( 1 ) ) );
|
|
}
|
|
|
|
|
|
/**
|
|
* Shrinks the capacity of a std::vector to fit its content.
|
|
*
|
|
* See Scott Meyer, Effective STL, Addison-Wesley, 2001, pp. 77--79.
|
|
*/
|
|
template< typename T >
|
|
void shrinkToFit( std::vector< T >& v ) {
|
|
std::vector< T >( v ).swap( v );
|
|
}
|
|
|
|
|
|
|
|
} // namespace OpenSteer
|
|
|
|
|
|
// ----------------------------------------------------------------------------
|
|
#endif // OPENSTEER_UTILITIES_H
|