383 lines
14 KiB
C
383 lines
14 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.
|
||
|
|
//
|
||
|
|
//
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
//
|
||
|
|
// Vec3: OpenSteer's generic type for 3d vectors
|
||
|
|
//
|
||
|
|
// This file defines the class Vec3, which is used throughout OpenSteer to
|
||
|
|
// manipulate 3d geometric data. It includes standard vector operations (like
|
||
|
|
// vector addition, subtraction, scale, dot, cross...) and more idiosyncratic
|
||
|
|
// utility functions.
|
||
|
|
//
|
||
|
|
// When integrating OpenSteer into a preexisting 3d application, it may be
|
||
|
|
// important to use the 3d vector type of that application. In that case Vec3
|
||
|
|
// can be changed to inherit from the preexisting application' vector type and
|
||
|
|
// to match the interface used by OpenSteer to the interface provided by the
|
||
|
|
// preexisting 3d vector type.
|
||
|
|
//
|
||
|
|
// 10-04-04 bk: put everything into the OpenSteer namespace
|
||
|
|
// 03-26-03 cwr: created to replace for Hiranabe-san's execellent but larger
|
||
|
|
// vecmath package (http://objectclub.esm.co.jp/vecmath/)
|
||
|
|
//
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
|
||
|
|
|
||
|
|
#ifndef OPENSTEER_VEC3_H
|
||
|
|
#define OPENSTEER_VEC3_H
|
||
|
|
|
||
|
|
|
||
|
|
#include "OpenSteer/Utilities.h" // for interpolate, etc.
|
||
|
|
|
||
|
|
|
||
|
|
namespace OpenSteer {
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
|
||
|
|
|
||
|
|
class Vec3
|
||
|
|
{
|
||
|
|
public:
|
||
|
|
|
||
|
|
// ----------------------------------------- generic 3d vector operations
|
||
|
|
|
||
|
|
// three-dimensional Cartesian coordinates
|
||
|
|
float x, y, z;
|
||
|
|
|
||
|
|
// constructors
|
||
|
|
Vec3 (void): x( 0.0f ), y( 0.0f ), z( 0.0f ) {}
|
||
|
|
Vec3 (float X, float Y, float Z) : x( X ), y( Y ), z( Z ) {}
|
||
|
|
|
||
|
|
// vector addition
|
||
|
|
Vec3 operator+ (const Vec3& v) const {return Vec3 (x+v.x, y+v.y, z+v.z);}
|
||
|
|
|
||
|
|
// vector subtraction
|
||
|
|
Vec3 operator- (const Vec3& v) const {return Vec3 (x-v.x, y-v.y, z-v.z);}
|
||
|
|
|
||
|
|
// unary minus
|
||
|
|
Vec3 operator- (void) const {return Vec3 (-x, -y, -z);}
|
||
|
|
|
||
|
|
// vector times scalar product (scale length of vector times argument)
|
||
|
|
Vec3 operator* (const float s) const {return Vec3 (x * s, y * s, z * s);}
|
||
|
|
|
||
|
|
// vector divided by a scalar (divide length of vector by argument)
|
||
|
|
Vec3 operator/ (const float s) const {return Vec3 (x / s, y / s, z / s);}
|
||
|
|
|
||
|
|
// dot product
|
||
|
|
float dot (const Vec3& v) const {return (x * v.x) + (y * v.y) + (z * v.z);}
|
||
|
|
|
||
|
|
// length
|
||
|
|
float length (void) const {return sqrtXXX (lengthSquared ());}
|
||
|
|
|
||
|
|
// length squared
|
||
|
|
float lengthSquared (void) const {return this->dot (*this);}
|
||
|
|
|
||
|
|
// normalize: returns normalized version (parallel to this, length = 1)
|
||
|
|
Vec3 normalize (void) const
|
||
|
|
{
|
||
|
|
// skip divide if length is zero
|
||
|
|
const float len = length ();
|
||
|
|
return (len>0) ? (*this)/len : (*this);
|
||
|
|
}
|
||
|
|
|
||
|
|
// cross product (modify "*this" to be A x B)
|
||
|
|
// [XXX side effecting -- deprecate this function? XXX]
|
||
|
|
void cross(const Vec3& a, const Vec3& b)
|
||
|
|
{
|
||
|
|
*this = Vec3 ((a.y * b.z) - (a.z * b.y),
|
||
|
|
(a.z * b.x) - (a.x * b.z),
|
||
|
|
(a.x * b.y) - (a.y * b.x));
|
||
|
|
}
|
||
|
|
|
||
|
|
// assignment
|
||
|
|
Vec3 operator= (const Vec3& v) {x=v.x; y=v.y; z=v.z; return *this;}
|
||
|
|
|
||
|
|
// set XYZ coordinates to given three floats
|
||
|
|
Vec3 set (const float _x, const float _y, const float _z)
|
||
|
|
{x = _x; y = _y; z = _z; return *this;}
|
||
|
|
|
||
|
|
// +=
|
||
|
|
Vec3 operator+= (const Vec3& v) {return *this = (*this + v);}
|
||
|
|
|
||
|
|
// -=
|
||
|
|
Vec3 operator-= (const Vec3& v) {return *this = (*this - v);}
|
||
|
|
|
||
|
|
// *=
|
||
|
|
Vec3 operator*= (const float& s) {return *this = (*this * s);}
|
||
|
|
|
||
|
|
|
||
|
|
Vec3 operator/=( float d ) { return *this = (*this / d); }
|
||
|
|
|
||
|
|
// equality/inequality
|
||
|
|
bool operator== (const Vec3& v) const {return x==v.x && y==v.y && z==v.z;}
|
||
|
|
bool operator!= (const Vec3& v) const {return !(*this == v);}
|
||
|
|
|
||
|
|
// @todo Remove - use @c distance from the Vec3Utilitites header instead.
|
||
|
|
// XXX experimental (4-1-03 cwr): is this the right approach? defining
|
||
|
|
// XXX "Vec3 distance (vec3, Vec3)" collided with STL's distance template.
|
||
|
|
static float distance (const Vec3& a, const Vec3& b){ return(a-b).length();}
|
||
|
|
|
||
|
|
// --------------------------- utility member functions used in OpenSteer
|
||
|
|
|
||
|
|
// return component of vector parallel to a unit basis vector
|
||
|
|
// (IMPORTANT NOTE: assumes "basis" has unit magnitude (length==1))
|
||
|
|
|
||
|
|
inline Vec3 parallelComponent (const Vec3& unitBasis) const
|
||
|
|
{
|
||
|
|
const float projection = this->dot (unitBasis);
|
||
|
|
return unitBasis * projection;
|
||
|
|
}
|
||
|
|
|
||
|
|
// return component of vector perpendicular to a unit basis vector
|
||
|
|
// (IMPORTANT NOTE: assumes "basis" has unit magnitude (length==1))
|
||
|
|
|
||
|
|
inline Vec3 perpendicularComponent (const Vec3& unitBasis) const
|
||
|
|
{
|
||
|
|
return (*this) - parallelComponent (unitBasis);
|
||
|
|
}
|
||
|
|
|
||
|
|
// clamps the length of a given vector to maxLength. If the vector is
|
||
|
|
// shorter its value is returned unaltered, if the vector is longer
|
||
|
|
// the value returned has length of maxLength and is paralle to the
|
||
|
|
// original input.
|
||
|
|
|
||
|
|
Vec3 truncateLength (const float maxLength) const
|
||
|
|
{
|
||
|
|
const float maxLengthSquared = maxLength * maxLength;
|
||
|
|
const float vecLengthSquared = this->lengthSquared ();
|
||
|
|
if (vecLengthSquared <= maxLengthSquared)
|
||
|
|
return *this;
|
||
|
|
else
|
||
|
|
return (*this) * (maxLength / sqrtXXX (vecLengthSquared));
|
||
|
|
}
|
||
|
|
|
||
|
|
// forces a 3d position onto the XZ (aka y=0) plane
|
||
|
|
|
||
|
|
Vec3 setYtoZero (void) const {return Vec3 (this->x, 0, this->z);}
|
||
|
|
|
||
|
|
// rotate this vector about the global Y (up) axis by the given angle
|
||
|
|
|
||
|
|
Vec3 rotateAboutGlobalY (float angle) const
|
||
|
|
{
|
||
|
|
const float s = sinXXX (angle);
|
||
|
|
const float c = cosXXX (angle);
|
||
|
|
return Vec3 ((this->x * c) + (this->z * s),
|
||
|
|
(this->y),
|
||
|
|
(this->z * c) - (this->x * s));
|
||
|
|
}
|
||
|
|
|
||
|
|
// version for caching sin/cos computation
|
||
|
|
Vec3 rotateAboutGlobalY (float angle, float& sin, float& cos) const
|
||
|
|
{
|
||
|
|
// is both are zero, they have not be initialized yet
|
||
|
|
if (sin==0 && cos==0)
|
||
|
|
{
|
||
|
|
sin = sinXXX (angle);
|
||
|
|
cos = cosXXX (angle);
|
||
|
|
}
|
||
|
|
return Vec3 ((this->x * cos) + (this->z * sin),
|
||
|
|
(this->y),
|
||
|
|
(this->z * cos) - (this->x * sin));
|
||
|
|
}
|
||
|
|
|
||
|
|
// if this position is outside sphere, push it back in by one diameter
|
||
|
|
|
||
|
|
Vec3 sphericalWrapAround (const Vec3& center, float radius)
|
||
|
|
{
|
||
|
|
const Vec3 offset = *this - center;
|
||
|
|
const float r = offset.length();
|
||
|
|
if (r > radius)
|
||
|
|
return *this + ((offset/r) * radius * -2);
|
||
|
|
else
|
||
|
|
return *this;
|
||
|
|
}
|
||
|
|
|
||
|
|
// names for frequently used vector constants
|
||
|
|
static const Vec3 zero;
|
||
|
|
static const Vec3 side;
|
||
|
|
static const Vec3 up;
|
||
|
|
static const Vec3 forward;
|
||
|
|
};
|
||
|
|
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
// scalar times vector product ("float * Vec3")
|
||
|
|
|
||
|
|
|
||
|
|
inline Vec3 operator* (float s, const Vec3& v) {return v*s;}
|
||
|
|
|
||
|
|
|
||
|
|
// return cross product a x b
|
||
|
|
inline Vec3 crossProduct(const Vec3& a, const Vec3& b)
|
||
|
|
{
|
||
|
|
Vec3 result((a.y * b.z) - (a.z * b.y),
|
||
|
|
(a.z * b.x) - (a.x * b.z),
|
||
|
|
(a.x * b.y) - (a.y * b.x));
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
// default character stream output method
|
||
|
|
|
||
|
|
#ifndef NOT_OPENSTEERDEMO // only when building OpenSteerDemo
|
||
|
|
|
||
|
|
inline std::ostream& operator<< (std::ostream& o, const Vec3& v)
|
||
|
|
{
|
||
|
|
return o << "(" << v.x << "," << v.y << "," << v.z << ")";
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif // NOT_OPENSTEERDEMO
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
// Returns a position randomly distributed inside a sphere of unit radius
|
||
|
|
// centered at the origin. Orientation will be random and length will range
|
||
|
|
// between 0 and 1
|
||
|
|
|
||
|
|
|
||
|
|
Vec3 RandomVectorInUnitRadiusSphere (void);
|
||
|
|
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
// Returns a position randomly distributed on a disk of unit radius
|
||
|
|
// on the XZ (Y=0) plane, centered at the origin. Orientation will be
|
||
|
|
// random and length will range between 0 and 1
|
||
|
|
|
||
|
|
|
||
|
|
Vec3 randomVectorOnUnitRadiusXZDisk (void);
|
||
|
|
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
// Returns a position randomly distributed on the surface of a sphere
|
||
|
|
// of unit radius centered at the origin. Orientation will be random
|
||
|
|
// and length will be 1
|
||
|
|
|
||
|
|
|
||
|
|
inline Vec3 RandomUnitVector (void)
|
||
|
|
{
|
||
|
|
return RandomVectorInUnitRadiusSphere().normalize();
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
// Returns a position randomly distributed on a circle of unit radius
|
||
|
|
// on the XZ (Y=0) plane, centered at the origin. Orientation will be
|
||
|
|
// random and length will be 1
|
||
|
|
|
||
|
|
|
||
|
|
inline Vec3 RandomUnitVectorOnXZPlane (void)
|
||
|
|
{
|
||
|
|
return RandomVectorInUnitRadiusSphere().setYtoZero().normalize();
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
// used by limitMaxDeviationAngle / limitMinDeviationAngle below
|
||
|
|
|
||
|
|
|
||
|
|
Vec3 vecLimitDeviationAngleUtility (const bool insideOrOutside,
|
||
|
|
const Vec3& source,
|
||
|
|
const float cosineOfConeAngle,
|
||
|
|
const Vec3& basis);
|
||
|
|
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
// Enforce an upper bound on the angle by which a given arbitrary vector
|
||
|
|
// diviates from a given reference direction (specified by a unit basis
|
||
|
|
// vector). The effect is to clip the "source" vector to be inside a cone
|
||
|
|
// defined by the basis and an angle.
|
||
|
|
|
||
|
|
|
||
|
|
inline Vec3 limitMaxDeviationAngle (const Vec3& source,
|
||
|
|
const float cosineOfConeAngle,
|
||
|
|
const Vec3& basis)
|
||
|
|
{
|
||
|
|
return vecLimitDeviationAngleUtility (true, // force source INSIDE cone
|
||
|
|
source,
|
||
|
|
cosineOfConeAngle,
|
||
|
|
basis);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
// Enforce a lower bound on the angle by which a given arbitrary vector
|
||
|
|
// diviates from a given reference direction (specified by a unit basis
|
||
|
|
// vector). The effect is to clip the "source" vector to be outside a cone
|
||
|
|
// defined by the basis and an angle.
|
||
|
|
|
||
|
|
|
||
|
|
inline Vec3 limitMinDeviationAngle (const Vec3& source,
|
||
|
|
const float cosineOfConeAngle,
|
||
|
|
const Vec3& basis)
|
||
|
|
{
|
||
|
|
return vecLimitDeviationAngleUtility (false, // force source OUTSIDE cone
|
||
|
|
source,
|
||
|
|
cosineOfConeAngle,
|
||
|
|
basis);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
// Returns the distance between a point and a line. The line is defined in
|
||
|
|
// terms of a point on the line ("lineOrigin") and a UNIT vector parallel to
|
||
|
|
// the line ("lineUnitTangent")
|
||
|
|
|
||
|
|
|
||
|
|
inline float distanceFromLine (const Vec3& point,
|
||
|
|
const Vec3& lineOrigin,
|
||
|
|
const Vec3& lineUnitTangent)
|
||
|
|
{
|
||
|
|
const Vec3 offset = point - lineOrigin;
|
||
|
|
const Vec3 perp = offset.perpendicularComponent (lineUnitTangent);
|
||
|
|
return perp.length();
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
// given a vector, return a vector perpendicular to it (note that this
|
||
|
|
// arbitrarily selects one of the infinitude of perpendicular vectors)
|
||
|
|
|
||
|
|
|
||
|
|
Vec3 findPerpendicularIn3d (const Vec3& direction);
|
||
|
|
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
// candidates for global utility functions
|
||
|
|
//
|
||
|
|
// dot
|
||
|
|
// cross
|
||
|
|
// length
|
||
|
|
// distance
|
||
|
|
// normalized
|
||
|
|
|
||
|
|
|
||
|
|
} // namespace OpenSteer
|
||
|
|
|
||
|
|
|
||
|
|
// ----------------------------------------------------------------------------
|
||
|
|
#endif // OPENSTEER_VEC3_H
|