mirror of
https://github.com/DrBeef/Raze.git
synced 2024-11-15 17:01:51 +00:00
13ea55b4cb
* Went the el cheapo route and added some floating point Build angle methods. As these and the integer build methods are really just refactoring aids, we'll come back to this in due course.
1679 lines
37 KiB
C++
1679 lines
37 KiB
C++
/*
|
|
** vectors.h
|
|
** Vector math routines.
|
|
**
|
|
**---------------------------------------------------------------------------
|
|
** Copyright 2005-2007 Randy Heit
|
|
** All rights reserved.
|
|
**
|
|
** Redistribution and use in source and binary forms, with or without
|
|
** modification, are permitted provided that the following conditions
|
|
** are met:
|
|
**
|
|
** 1. Redistributions of source code must retain the above copyright
|
|
** notice, this list of conditions and the following disclaimer.
|
|
** 2. Redistributions in binary form must reproduce the above copyright
|
|
** notice, this list of conditions and the following disclaimer in the
|
|
** documentation and/or other materials provided with the distribution.
|
|
** 3. The name of the author may not be used to endorse or promote products
|
|
** derived from this software without specific prior written permission.
|
|
**
|
|
** THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
|
|
** IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
|
|
** OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
|
|
** IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
** INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
|
|
** NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
|
|
** DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
|
|
** THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
|
|
** THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
**---------------------------------------------------------------------------
|
|
**
|
|
** Since C++ doesn't let me add completely new operators, the following two
|
|
** are overloaded for vectors:
|
|
**
|
|
** | dot product
|
|
** ^ cross product
|
|
*/
|
|
|
|
#ifndef VECTORS_H
|
|
#define VECTORS_H
|
|
|
|
#include <cstddef>
|
|
#include <math.h>
|
|
#include <float.h>
|
|
#include <string.h>
|
|
#include "xs_Float.h"
|
|
#include "math/cmath.h"
|
|
|
|
|
|
#define EQUAL_EPSILON (1/65536.)
|
|
|
|
// make this a local inline function to avoid any dependencies on other headers and not pollute the global namespace
|
|
namespace pi
|
|
{
|
|
inline constexpr double pi() { return 3.14159265358979323846; }
|
|
inline constexpr double pif() { return 3.14159265358979323846f; }
|
|
}
|
|
|
|
|
|
|
|
template<class vec_t> struct TVector3;
|
|
template<class vec_t> struct TRotator;
|
|
template<class vec_t> struct TAngle;
|
|
|
|
template<class vec_t>
|
|
struct TVector2
|
|
{
|
|
vec_t X, Y;
|
|
|
|
TVector2() = default;
|
|
|
|
TVector2 (vec_t a, vec_t b)
|
|
: X(a), Y(b)
|
|
{
|
|
}
|
|
|
|
TVector2(const TVector2 &other) = default;
|
|
|
|
TVector2 (const TVector3<vec_t> &other) // Copy the X and Y from the 3D vector and discard the Z
|
|
: X(other.X), Y(other.Y)
|
|
{
|
|
}
|
|
|
|
TVector2(vec_t *o)
|
|
: X(o[0]), Y(o[1])
|
|
{
|
|
}
|
|
|
|
void Zero()
|
|
{
|
|
Y = X = 0;
|
|
}
|
|
|
|
bool isZero() const
|
|
{
|
|
return X == 0 && Y == 0;
|
|
}
|
|
|
|
TVector2 &operator= (const TVector2 &other) = default;
|
|
|
|
// Access X and Y as an array
|
|
vec_t &operator[] (int index)
|
|
{
|
|
return index == 0 ? X : Y;
|
|
}
|
|
|
|
const vec_t &operator[] (int index) const
|
|
{
|
|
return index == 0 ? X : Y;
|
|
}
|
|
|
|
// Test for equality
|
|
bool operator== (const TVector2 &other) const
|
|
{
|
|
return X == other.X && Y == other.Y;
|
|
}
|
|
|
|
// Test for inequality
|
|
bool operator!= (const TVector2 &other) const
|
|
{
|
|
return X != other.X || Y != other.Y;
|
|
}
|
|
|
|
// Test for approximate equality
|
|
bool ApproximatelyEquals (const TVector2 &other) const
|
|
{
|
|
return fabs(X - other.X) < EQUAL_EPSILON && fabs(Y - other.Y) < EQUAL_EPSILON;
|
|
}
|
|
|
|
// Test for approximate inequality
|
|
bool DoesNotApproximatelyEqual (const TVector2 &other) const
|
|
{
|
|
return fabs(X - other.X) >= EQUAL_EPSILON || fabs(Y - other.Y) >= EQUAL_EPSILON;
|
|
}
|
|
|
|
// Unary negation
|
|
TVector2 operator- () const
|
|
{
|
|
return TVector2(-X, -Y);
|
|
}
|
|
|
|
// Scalar addition
|
|
#if 0
|
|
TVector2 &operator+= (double scalar)
|
|
{
|
|
X += scalar, Y += scalar;
|
|
return *this;
|
|
}
|
|
#endif
|
|
|
|
friend TVector2 operator+ (const TVector2 &v, vec_t scalar)
|
|
{
|
|
return TVector2(v.X + scalar, v.Y + scalar);
|
|
}
|
|
|
|
friend TVector2 operator+ (vec_t scalar, const TVector2 &v)
|
|
{
|
|
return TVector2(v.X + scalar, v.Y + scalar);
|
|
}
|
|
|
|
// Scalar subtraction
|
|
TVector2 &operator-= (vec_t scalar)
|
|
{
|
|
X -= scalar, Y -= scalar;
|
|
return *this;
|
|
}
|
|
|
|
TVector2 operator- (vec_t scalar) const
|
|
{
|
|
return TVector2(X - scalar, Y - scalar);
|
|
}
|
|
|
|
// Scalar multiplication
|
|
TVector2 &operator*= (vec_t scalar)
|
|
{
|
|
X *= scalar, Y *= scalar;
|
|
return *this;
|
|
}
|
|
|
|
friend TVector2 operator* (const TVector2 &v, vec_t scalar)
|
|
{
|
|
return TVector2(v.X * scalar, v.Y * scalar);
|
|
}
|
|
|
|
friend TVector2 operator* (vec_t scalar, const TVector2 &v)
|
|
{
|
|
return TVector2(v.X * scalar, v.Y * scalar);
|
|
}
|
|
|
|
// Scalar division
|
|
TVector2 &operator/= (vec_t scalar)
|
|
{
|
|
scalar = 1 / scalar, X *= scalar, Y *= scalar;
|
|
return *this;
|
|
}
|
|
|
|
TVector2 operator/ (vec_t scalar) const
|
|
{
|
|
scalar = 1 / scalar;
|
|
return TVector2(X * scalar, Y * scalar);
|
|
}
|
|
|
|
// Vector addition
|
|
TVector2 &operator+= (const TVector2 &other)
|
|
{
|
|
X += other.X, Y += other.Y;
|
|
return *this;
|
|
}
|
|
|
|
TVector2 operator+ (const TVector2 &other) const
|
|
{
|
|
return TVector2(X + other.X, Y + other.Y);
|
|
}
|
|
|
|
// Vector subtraction
|
|
TVector2 &operator-= (const TVector2 &other)
|
|
{
|
|
X -= other.X, Y -= other.Y;
|
|
return *this;
|
|
}
|
|
|
|
TVector2 operator- (const TVector2 &other) const
|
|
{
|
|
return TVector2(X - other.X, Y - other.Y);
|
|
}
|
|
|
|
// Vector length
|
|
vec_t Length() const
|
|
{
|
|
return (vec_t)g_sqrt (X*X + Y*Y);
|
|
}
|
|
|
|
vec_t LengthSquared() const
|
|
{
|
|
return X*X + Y*Y;
|
|
}
|
|
|
|
// Return a unit vector facing the same direction as this one
|
|
TVector2 Unit() const
|
|
{
|
|
vec_t len = Length();
|
|
if (len != 0) len = 1 / len;
|
|
return *this * len;
|
|
}
|
|
|
|
// Scales this vector into a unit vector. Returns the old length
|
|
vec_t MakeUnit()
|
|
{
|
|
vec_t len, ilen;
|
|
len = ilen = Length();
|
|
if (ilen != 0) ilen = 1 / ilen;
|
|
*this *= ilen;
|
|
return len;
|
|
}
|
|
|
|
// Resizes this vector to be the specified length (if it is not 0)
|
|
TVector2 &MakeResize(double len)
|
|
{
|
|
double scale = len / Length();
|
|
X = vec_t(X * scale);
|
|
Y = vec_t(Y * scale);
|
|
return *this;
|
|
}
|
|
|
|
|
|
// Dot product
|
|
vec_t operator | (const TVector2 &other) const
|
|
{
|
|
return X*other.X + Y*other.Y;
|
|
}
|
|
|
|
// Returns the angle that the ray (0,0)-(X,Y) faces
|
|
TAngle<vec_t> Angle() const;
|
|
|
|
// Returns a rotated vector. angle is in degrees.
|
|
TVector2 Rotated (double angle)
|
|
{
|
|
double cosval = g_cosdeg (angle);
|
|
double sinval = g_sindeg (angle);
|
|
return TVector2(X*cosval - Y*sinval, Y*cosval + X*sinval);
|
|
}
|
|
|
|
// Returns a rotated vector. angle is in degrees.
|
|
template<class T>
|
|
TVector2 Rotated(TAngle<T> angle)
|
|
{
|
|
double cosval = angle.Cos();
|
|
double sinval = angle.Sin();
|
|
return TVector2(X*cosval - Y*sinval, Y*cosval + X*sinval);
|
|
}
|
|
|
|
// Returns a vector rotated 90 degrees clockwise.
|
|
TVector2 Rotated90CW()
|
|
{
|
|
return TVector2(Y, -X);
|
|
}
|
|
|
|
// Returns a vector rotated 90 degrees counterclockwise.
|
|
TVector2 Rotated90CCW()
|
|
{
|
|
return TVector2(-Y, X);
|
|
}
|
|
};
|
|
|
|
template<class vec_t>
|
|
struct TVector3
|
|
{
|
|
typedef TVector2<vec_t> Vector2;
|
|
|
|
vec_t X, Y, Z;
|
|
|
|
TVector3() = default;
|
|
|
|
TVector3 (vec_t a, vec_t b, vec_t c)
|
|
: X(a), Y(b), Z(c)
|
|
{
|
|
}
|
|
|
|
TVector3(vec_t *o)
|
|
: X(o[0]), Y(o[1]), Z(o[2])
|
|
{
|
|
}
|
|
|
|
TVector3(std::nullptr_t nul) = delete;
|
|
|
|
TVector3(const TVector3 &other) = default;
|
|
|
|
TVector3 (const Vector2 &xy, vec_t z)
|
|
: X(xy.X), Y(xy.Y), Z(z)
|
|
{
|
|
}
|
|
|
|
TVector3 (const TRotator<vec_t> &rot);
|
|
|
|
void Zero()
|
|
{
|
|
Z = Y = X = 0;
|
|
}
|
|
|
|
bool isZero() const
|
|
{
|
|
return X == 0 && Y == 0 && Z == 0;
|
|
}
|
|
|
|
TVector3 plusZ(double z)
|
|
{
|
|
return { X, Y, Z + z };
|
|
}
|
|
|
|
TVector3 &operator= (const TVector3 &other) = default;
|
|
|
|
// Access X and Y and Z as an array
|
|
vec_t &operator[] (int index)
|
|
{
|
|
return index == 0 ? X : index == 1 ? Y : Z;
|
|
}
|
|
|
|
const vec_t &operator[] (int index) const
|
|
{
|
|
return index == 0 ? X : index == 1 ? Y : Z;
|
|
}
|
|
|
|
// Test for equality
|
|
bool operator== (const TVector3 &other) const
|
|
{
|
|
return X == other.X && Y == other.Y && Z == other.Z;
|
|
}
|
|
|
|
// Test for inequality
|
|
bool operator!= (const TVector3 &other) const
|
|
{
|
|
return X != other.X || Y != other.Y || Z != other.Z;
|
|
}
|
|
|
|
// Test for approximate equality
|
|
bool ApproximatelyEquals (const TVector3 &other) const
|
|
{
|
|
return fabs(X - other.X) < EQUAL_EPSILON && fabs(Y - other.Y) < EQUAL_EPSILON && fabs(Z - other.Z) < EQUAL_EPSILON;
|
|
}
|
|
|
|
// Test for approximate inequality
|
|
bool DoesNotApproximatelyEqual (const TVector3 &other) const
|
|
{
|
|
return fabs(X - other.X) >= EQUAL_EPSILON || fabs(Y - other.Y) >= EQUAL_EPSILON || fabs(Z - other.Z) >= EQUAL_EPSILON;
|
|
}
|
|
|
|
// Unary negation
|
|
TVector3 operator- () const
|
|
{
|
|
return TVector3(-X, -Y, -Z);
|
|
}
|
|
|
|
// Scalar addition
|
|
#if 0
|
|
TVector3 &operator+= (vec_t scalar)
|
|
{
|
|
X += scalar, Y += scalar, Z += scalar;
|
|
return *this;
|
|
}
|
|
#endif
|
|
|
|
friend TVector3 operator+ (const TVector3 &v, vec_t scalar)
|
|
{
|
|
return TVector3(v.X + scalar, v.Y + scalar, v.Z + scalar);
|
|
}
|
|
|
|
friend TVector3 operator+ (vec_t scalar, const TVector3 &v)
|
|
{
|
|
return TVector3(v.X + scalar, v.Y + scalar, v.Z + scalar);
|
|
}
|
|
|
|
// Scalar subtraction
|
|
TVector3 &operator-= (vec_t scalar)
|
|
{
|
|
X -= scalar, Y -= scalar, Z -= scalar;
|
|
return *this;
|
|
}
|
|
|
|
TVector3 operator- (vec_t scalar) const
|
|
{
|
|
return TVector3(X - scalar, Y - scalar, Z - scalar);
|
|
}
|
|
|
|
// Scalar multiplication
|
|
TVector3 &operator*= (vec_t scalar)
|
|
{
|
|
X = vec_t(X *scalar), Y = vec_t(Y * scalar), Z = vec_t(Z * scalar);
|
|
return *this;
|
|
}
|
|
|
|
friend TVector3 operator* (const TVector3 &v, vec_t scalar)
|
|
{
|
|
return TVector3(v.X * scalar, v.Y * scalar, v.Z * scalar);
|
|
}
|
|
|
|
friend TVector3 operator* (vec_t scalar, const TVector3 &v)
|
|
{
|
|
return TVector3(v.X * scalar, v.Y * scalar, v.Z * scalar);
|
|
}
|
|
|
|
// Scalar division
|
|
TVector3 &operator/= (vec_t scalar)
|
|
{
|
|
scalar = 1 / scalar, X = vec_t(X * scalar), Y = vec_t(Y * scalar), Z = vec_t(Z * scalar);
|
|
return *this;
|
|
}
|
|
|
|
TVector3 operator/ (vec_t scalar) const
|
|
{
|
|
scalar = 1 / scalar;
|
|
return TVector3(X * scalar, Y * scalar, Z * scalar);
|
|
}
|
|
|
|
// Vector addition
|
|
TVector3 &operator+= (const TVector3 &other)
|
|
{
|
|
X += other.X, Y += other.Y, Z += other.Z;
|
|
return *this;
|
|
}
|
|
|
|
TVector3 operator+ (const TVector3 &other) const
|
|
{
|
|
return TVector3(X + other.X, Y + other.Y, Z + other.Z);
|
|
}
|
|
|
|
// Vector subtraction
|
|
TVector3 &operator-= (const TVector3 &other)
|
|
{
|
|
X -= other.X, Y -= other.Y, Z -= other.Z;
|
|
return *this;
|
|
}
|
|
|
|
TVector3 operator- (const TVector3 &other) const
|
|
{
|
|
return TVector3(X - other.X, Y - other.Y, Z - other.Z);
|
|
}
|
|
|
|
// Add a 2D vector to this 3D vector, leaving Z unchanged.
|
|
TVector3 &operator+= (const Vector2 &other)
|
|
{
|
|
X += other.X, Y += other.Y;
|
|
return *this;
|
|
}
|
|
|
|
// Subtract a 2D vector from this 3D vector, leaving Z unchanged.
|
|
TVector3 &operator-= (const Vector2 &other)
|
|
{
|
|
X -= other.X, Y -= other.Y;
|
|
return *this;
|
|
}
|
|
|
|
// returns the XY fields as a 2D-vector.
|
|
Vector2 XY() const
|
|
{
|
|
return{ X, Y };
|
|
}
|
|
|
|
// Add a 3D vector and a 2D vector.
|
|
friend TVector3 operator+ (const TVector3 &v3, const Vector2 &v2)
|
|
{
|
|
return TVector3(v3.X + v2.X, v3.Y + v2.Y, v3.Z);
|
|
}
|
|
|
|
friend TVector3 operator- (const TVector3 &v3, const Vector2 &v2)
|
|
{
|
|
return TVector3(v3.X - v2.X, v3.Y - v2.Y, v3.Z);
|
|
}
|
|
|
|
friend Vector2 operator+ (const Vector2 &v2, const TVector3 &v3)
|
|
{
|
|
return Vector2(v2.X + v3.X, v2.Y + v3.Y);
|
|
}
|
|
|
|
// Subtract a 3D vector and a 2D vector.
|
|
// Discards the Z component of the 3D vector and returns a 2D vector.
|
|
friend Vector2 operator- (const TVector2<vec_t> &v2, const TVector3 &v3)
|
|
{
|
|
return Vector2(v2.X - v3.X, v2.Y - v3.Y);
|
|
}
|
|
|
|
void GetRightUp(TVector3 &right, TVector3 &up)
|
|
{
|
|
TVector3 n(X, Y, Z);
|
|
TVector3 fn(fabs(n.X), fabs(n.Y), fabs(n.Z));
|
|
int major = 0;
|
|
|
|
if (fn[1] > fn[major]) major = 1;
|
|
if (fn[2] > fn[major]) major = 2;
|
|
|
|
// build right vector by hand
|
|
if (fabs(fn[0] - 1.0f) < FLT_EPSILON || fabs(fn[1] - 1.0f) < FLT_EPSILON || fabs(fn[2] - 1.0f) < FLT_EPSILON)
|
|
{
|
|
if (major == 0 && n[0] > 0.f)
|
|
{
|
|
right = { 0.f, 0.f, -1.f };
|
|
}
|
|
else if (major == 0)
|
|
{
|
|
right = { 0.f, 0.f, 1.f };
|
|
}
|
|
else if (major == 1 || (major == 2 && n[2] > 0.f))
|
|
{
|
|
right = { 1.f, 0.f, 0.f };
|
|
}
|
|
// Unconditional to ease static analysis
|
|
else // major == 2 && n[2] <= 0.0f
|
|
{
|
|
right = { -1.f, 0.f, 0.f };
|
|
}
|
|
}
|
|
else
|
|
{
|
|
static TVector3 axis[3] =
|
|
{
|
|
{ 1.0f, 0.0f, 0.0f },
|
|
{ 0.0f, 1.0f, 0.0f },
|
|
{ 0.0f, 0.0f, 1.0f }
|
|
};
|
|
|
|
right = axis[major] ^ n;
|
|
}
|
|
|
|
up = n ^right;
|
|
right.MakeUnit();;
|
|
up.MakeUnit();
|
|
}
|
|
|
|
|
|
// Returns the angle (in radians) that the ray (0,0)-(X,Y) faces
|
|
TAngle<vec_t> Angle() const;
|
|
TAngle<vec_t> Pitch() const;
|
|
|
|
// Vector length
|
|
double Length() const
|
|
{
|
|
return g_sqrt (X*X + Y*Y + Z*Z);
|
|
}
|
|
|
|
double LengthSquared() const
|
|
{
|
|
return X*X + Y*Y + Z*Z;
|
|
}
|
|
|
|
// Return a unit vector facing the same direction as this one
|
|
TVector3 Unit() const
|
|
{
|
|
double len = Length();
|
|
if (len != 0) len = 1 / len;
|
|
return *this * (vec_t)len;
|
|
}
|
|
|
|
// Scales this vector into a unit vector
|
|
void MakeUnit()
|
|
{
|
|
double len = Length();
|
|
if (len != 0) len = 1 / len;
|
|
*this *= (vec_t)len;
|
|
}
|
|
|
|
// Resizes this vector to be the specified length (if it is not 0)
|
|
TVector3 &MakeResize(double len)
|
|
{
|
|
double vlen = Length();
|
|
if (vlen != 0.)
|
|
{
|
|
double scale = len / vlen;
|
|
X = vec_t(X * scale);
|
|
Y = vec_t(Y * scale);
|
|
Z = vec_t(Z * scale);
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
TVector3 Resized(double len)
|
|
{
|
|
double vlen = Length();
|
|
if (vlen != 0.)
|
|
{
|
|
double scale = len / vlen;
|
|
return{ vec_t(X * scale), vec_t(Y * scale), vec_t(Z * scale) };
|
|
}
|
|
else
|
|
{
|
|
return *this;
|
|
}
|
|
}
|
|
|
|
// Dot product
|
|
vec_t operator | (const TVector3 &other) const
|
|
{
|
|
return X*other.X + Y*other.Y + Z*other.Z;
|
|
}
|
|
|
|
// Cross product
|
|
TVector3 operator ^ (const TVector3 &other) const
|
|
{
|
|
return TVector3(Y*other.Z - Z*other.Y,
|
|
Z*other.X - X*other.Z,
|
|
X*other.Y - Y*other.X);
|
|
}
|
|
|
|
TVector3 &operator ^= (const TVector3 &other)
|
|
{
|
|
*this = *this ^ other;
|
|
return *this;
|
|
}
|
|
};
|
|
|
|
template<class vec_t>
|
|
struct TVector4
|
|
{
|
|
typedef TVector3<vec_t> Vector3;
|
|
|
|
vec_t X, Y, Z, W;
|
|
|
|
TVector4() = default;
|
|
|
|
TVector4(vec_t a, vec_t b, vec_t c, vec_t d)
|
|
: X(a), Y(b), Z(c), W(d)
|
|
{
|
|
}
|
|
|
|
TVector4(vec_t *o)
|
|
: X(o[0]), Y(o[1]), Z(o[2]), W(o[3])
|
|
{
|
|
}
|
|
|
|
TVector4(const TVector4 &other) = default;
|
|
|
|
TVector4(const Vector3 &xyz, vec_t w)
|
|
: X(xyz.X), Y(xyz.Y), Z(xyz.Z), W(w)
|
|
{
|
|
}
|
|
|
|
void Zero()
|
|
{
|
|
Z = Y = X = W = 0;
|
|
}
|
|
|
|
bool isZero() const
|
|
{
|
|
return X == 0 && Y == 0 && Z == 0 && W == 0;
|
|
}
|
|
|
|
TVector4 &operator= (const TVector4 &other) = default;
|
|
|
|
// Access X and Y and Z as an array
|
|
vec_t &operator[] (int index)
|
|
{
|
|
return (&X)[index];
|
|
}
|
|
|
|
const vec_t &operator[] (int index) const
|
|
{
|
|
return (&X)[index];
|
|
}
|
|
|
|
// Test for equality
|
|
bool operator== (const TVector4 &other) const
|
|
{
|
|
return X == other.X && Y == other.Y && Z == other.Z && W == other.W;
|
|
}
|
|
|
|
// Test for inequality
|
|
bool operator!= (const TVector4 &other) const
|
|
{
|
|
return X != other.X || Y != other.Y || Z != other.Z || W != other.W;
|
|
}
|
|
|
|
// Test for approximate equality
|
|
bool ApproximatelyEquals(const TVector4 &other) const
|
|
{
|
|
return fabs(X - other.X) < EQUAL_EPSILON && fabs(Y - other.Y) < EQUAL_EPSILON && fabs(Z - other.Z) < EQUAL_EPSILON && fabs(W - other.W) < EQUAL_EPSILON;
|
|
}
|
|
|
|
// Test for approximate inequality
|
|
bool DoesNotApproximatelyEqual(const TVector4 &other) const
|
|
{
|
|
return fabs(X - other.X) >= EQUAL_EPSILON || fabs(Y - other.Y) >= EQUAL_EPSILON || fabs(Z - other.Z) >= EQUAL_EPSILON || fabs(W - other.W) >= EQUAL_EPSILON;
|
|
}
|
|
|
|
// Unary negation
|
|
TVector4 operator- () const
|
|
{
|
|
return TVector4(-X, -Y, -Z, -W);
|
|
}
|
|
|
|
// Scalar addition
|
|
TVector4 &operator+= (vec_t scalar)
|
|
{
|
|
X += scalar, Y += scalar, Z += scalar; W += scalar;
|
|
return *this;
|
|
}
|
|
|
|
friend TVector4 operator+ (const TVector4 &v, vec_t scalar)
|
|
{
|
|
return TVector4(v.X + scalar, v.Y + scalar, v.Z + scalar, v.W + scalar);
|
|
}
|
|
|
|
friend TVector4 operator+ (vec_t scalar, const TVector4 &v)
|
|
{
|
|
return TVector4(v.X + scalar, v.Y + scalar, v.Z + scalar, v.W + scalar);
|
|
}
|
|
|
|
// Scalar subtraction
|
|
TVector4 &operator-= (vec_t scalar)
|
|
{
|
|
X -= scalar, Y -= scalar, Z -= scalar, W -= scalar;
|
|
return *this;
|
|
}
|
|
|
|
TVector4 operator- (vec_t scalar) const
|
|
{
|
|
return TVector4(X - scalar, Y - scalar, Z - scalar, W - scalar);
|
|
}
|
|
|
|
// Scalar multiplication
|
|
TVector4 &operator*= (vec_t scalar)
|
|
{
|
|
X = vec_t(X *scalar), Y = vec_t(Y * scalar), Z = vec_t(Z * scalar), W = vec_t(W * scalar);
|
|
return *this;
|
|
}
|
|
|
|
friend TVector4 operator* (const TVector4 &v, vec_t scalar)
|
|
{
|
|
return TVector4(v.X * scalar, v.Y * scalar, v.Z * scalar, v.W * scalar);
|
|
}
|
|
|
|
friend TVector4 operator* (vec_t scalar, const TVector4 &v)
|
|
{
|
|
return TVector4(v.X * scalar, v.Y * scalar, v.Z * scalar, v.W * scalar);
|
|
}
|
|
|
|
// Scalar division
|
|
TVector4 &operator/= (vec_t scalar)
|
|
{
|
|
scalar = 1 / scalar, X = vec_t(X * scalar), Y = vec_t(Y * scalar), Z = vec_t(Z * scalar), W = vec_t(W * scalar);
|
|
return *this;
|
|
}
|
|
|
|
TVector4 operator/ (vec_t scalar) const
|
|
{
|
|
scalar = 1 / scalar;
|
|
return TVector4(X * scalar, Y * scalar, Z * scalar, W * scalar);
|
|
}
|
|
|
|
// Vector addition
|
|
TVector4 &operator+= (const TVector4 &other)
|
|
{
|
|
X += other.X, Y += other.Y, Z += other.Z, W += other.W;
|
|
return *this;
|
|
}
|
|
|
|
TVector4 operator+ (const TVector4 &other) const
|
|
{
|
|
return TVector4(X + other.X, Y + other.Y, Z + other.Z, W + other.W);
|
|
}
|
|
|
|
// Vector subtraction
|
|
TVector4 &operator-= (const TVector4 &other)
|
|
{
|
|
X -= other.X, Y -= other.Y, Z -= other.Z, W -= other.W;
|
|
return *this;
|
|
}
|
|
|
|
TVector4 operator- (const TVector4 &other) const
|
|
{
|
|
return TVector4(X - other.X, Y - other.Y, Z - other.Z, W - other.W);
|
|
}
|
|
|
|
// Add a 3D vector to this 4D vector, leaving W unchanged.
|
|
TVector4 &operator+= (const Vector3 &other)
|
|
{
|
|
X += other.X, Y += other.Y, Z += other.Z;
|
|
return *this;
|
|
}
|
|
|
|
// Subtract a 3D vector from this 4D vector, leaving W unchanged.
|
|
TVector4 &operator-= (const Vector3 &other)
|
|
{
|
|
X -= other.X, Y -= other.Y, Z -= other.Z;
|
|
return *this;
|
|
}
|
|
|
|
// returns the XYZ fields as a 3D-vector.
|
|
Vector3 XYZ() const
|
|
{
|
|
return{ X, Y, Z };
|
|
}
|
|
|
|
// Add a 4D vector and a 3D vector.
|
|
friend TVector4 operator+ (const TVector4 &v4, const Vector3 &v3)
|
|
{
|
|
return TVector4(v4.X + v3.X, v4.Y + v3.Y, v4.Z + v3.Z, v4.W);
|
|
}
|
|
|
|
friend TVector4 operator- (const TVector4 &v4, const Vector3 &v3)
|
|
{
|
|
return TVector4(v4.X - v3.X, v4.Y - v3.Y, v4.Z - v3.Z, v4.W);
|
|
}
|
|
|
|
friend Vector3 operator+ (const Vector3 &v3, const TVector4 &v4)
|
|
{
|
|
return Vector3(v3.X + v4.X, v3.Y + v4.Y, v3.Z + v4.Z);
|
|
}
|
|
|
|
// Subtract a 4D vector and a 3D vector.
|
|
// Discards the W component of the 4D vector and returns a 3D vector.
|
|
friend Vector3 operator- (const TVector3<vec_t> &v3, const TVector4 &v4)
|
|
{
|
|
return Vector3(v3.X - v4.X, v3.Y - v4.Y, v3.Z - v4.Z);
|
|
}
|
|
|
|
// Vector length
|
|
double Length() const
|
|
{
|
|
return g_sqrt(X*X + Y*Y + Z*Z + W*W);
|
|
}
|
|
|
|
double LengthSquared() const
|
|
{
|
|
return X*X + Y*Y + Z*Z + W*W;
|
|
}
|
|
|
|
// Return a unit vector facing the same direction as this one
|
|
TVector4 Unit() const
|
|
{
|
|
double len = Length();
|
|
if (len != 0) len = 1 / len;
|
|
return *this * (vec_t)len;
|
|
}
|
|
|
|
// Scales this vector into a unit vector
|
|
void MakeUnit()
|
|
{
|
|
double len = Length();
|
|
if (len != 0) len = 1 / len;
|
|
*this *= (vec_t)len;
|
|
}
|
|
|
|
// Resizes this vector to be the specified length (if it is not 0)
|
|
TVector4 &MakeResize(double len)
|
|
{
|
|
double vlen = Length();
|
|
if (vlen != 0.)
|
|
{
|
|
double scale = len / vlen;
|
|
X = vec_t(X * scale);
|
|
Y = vec_t(Y * scale);
|
|
Z = vec_t(Z * scale);
|
|
W = vec_t(W * scale);
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
TVector4 Resized(double len)
|
|
{
|
|
double vlen = Length();
|
|
if (vlen != 0.)
|
|
{
|
|
double scale = len / vlen;
|
|
return{ vec_t(X * scale), vec_t(Y * scale), vec_t(Z * scale), vec_t(W * scale) };
|
|
}
|
|
else
|
|
{
|
|
return *this;
|
|
}
|
|
}
|
|
|
|
// Dot product
|
|
vec_t operator | (const TVector4 &other) const
|
|
{
|
|
return X*other.X + Y*other.Y + Z*other.Z + W*other.W;
|
|
}
|
|
};
|
|
|
|
template<class vec_t>
|
|
struct TMatrix3x3
|
|
{
|
|
typedef TVector3<vec_t> Vector3;
|
|
|
|
vec_t Cells[3][3];
|
|
|
|
TMatrix3x3() = default;
|
|
TMatrix3x3(const TMatrix3x3 &other) = default;
|
|
TMatrix3x3& operator=(const TMatrix3x3& other) = default;
|
|
|
|
TMatrix3x3(const Vector3 &row1, const Vector3 &row2, const Vector3 &row3)
|
|
{
|
|
(*this)[0] = row1;
|
|
(*this)[1] = row2;
|
|
(*this)[2] = row3;
|
|
}
|
|
|
|
// Construct a rotation matrix about an arbitrary axis.
|
|
// (The axis vector must be normalized.)
|
|
TMatrix3x3(const Vector3 &axis, double radians)
|
|
{
|
|
double c = g_cos(radians), s = g_sin(radians), t = 1 - c;
|
|
/* In comments: A more readable version of the matrix setup.
|
|
This was found in Diana Gruber's article "The Mathematics of the
|
|
3D Rotation Matrix" at <http://www.makegames.com/3drotation/> and is
|
|
attributed to Graphics Gems (Glassner, Academic Press, 1990).
|
|
|
|
Cells[0][0] = t*axis.X*axis.X + c;
|
|
Cells[0][1] = t*axis.X*axis.Y - s*axis.Z;
|
|
Cells[0][2] = t*axis.X*axis.Z + s*axis.Y;
|
|
|
|
Cells[1][0] = t*axis.Y*axis.X + s*axis.Z;
|
|
Cells[1][1] = t*axis.Y*axis.Y + c;
|
|
Cells[1][2] = t*axis.Y*axis.Z - s*axis.X;
|
|
|
|
Cells[2][0] = t*axis.Z*axis.X - s*axis.Y;
|
|
Cells[2][1] = t*axis.Z*axis.Y + s*axis.X;
|
|
Cells[2][2] = t*axis.Z*axis.Z + c;
|
|
|
|
Outside comments: A faster version with only 10 (not 24) multiplies.
|
|
*/
|
|
double sx = s*axis.X, sy = s*axis.Y, sz = s*axis.Z;
|
|
double tx, ty, txx, tyy, u, v;
|
|
|
|
tx = t*axis.X;
|
|
Cells[0][0] = vec_t( (txx=tx*axis.X) + c );
|
|
Cells[0][1] = vec_t( (u=tx*axis.Y) - sz);
|
|
Cells[0][2] = vec_t( (v=tx*axis.Z) + sy);
|
|
|
|
ty = t*axis.Y;
|
|
Cells[1][0] = vec_t( u + sz);
|
|
Cells[1][1] = vec_t( (tyy=ty*axis.Y) + c );
|
|
Cells[1][2] = vec_t( (u=ty*axis.Z) - sx);
|
|
|
|
Cells[2][0] = vec_t( v - sy);
|
|
Cells[2][1] = vec_t( u + sx);
|
|
Cells[2][2] = vec_t( (t-txx-tyy) + c );
|
|
}
|
|
|
|
TMatrix3x3(const Vector3 &axis, double c/*cosine*/, double s/*sine*/)
|
|
{
|
|
double t = 1 - c;
|
|
double sx = s*axis.X, sy = s*axis.Y, sz = s*axis.Z;
|
|
double tx, ty, txx, tyy, u, v;
|
|
|
|
tx = t*axis.X;
|
|
Cells[0][0] = vec_t( (txx=tx*axis.X) + c );
|
|
Cells[0][1] = vec_t( (u=tx*axis.Y) - sz);
|
|
Cells[0][2] = vec_t( (v=tx*axis.Z) + sy);
|
|
|
|
ty = t*axis.Y;
|
|
Cells[1][0] = vec_t( u + sz);
|
|
Cells[1][1] = vec_t( (tyy=ty*axis.Y) + c );
|
|
Cells[1][2] = vec_t( (u=ty*axis.Z) - sx);
|
|
|
|
Cells[2][0] = vec_t( v - sy);
|
|
Cells[2][1] = vec_t( u + sx);
|
|
Cells[2][2] = vec_t( (t-txx-tyy) + c );
|
|
}
|
|
|
|
TMatrix3x3(const Vector3 &axis, TAngle<vec_t> degrees);
|
|
|
|
static TMatrix3x3 Rotate2D(double radians)
|
|
{
|
|
double c = g_cos(radians);
|
|
double s = g_sin(radians);
|
|
TMatrix3x3 ret;
|
|
ret.Cells[0][0] = c; ret.Cells[0][1] = -s; ret.Cells[0][2] = 0;
|
|
ret.Cells[1][0] = s; ret.Cells[1][1] = c; ret.Cells[1][2] = 0;
|
|
ret.Cells[2][0] = 0; ret.Cells[2][1] = 0; ret.Cells[2][2] = 1;
|
|
return ret;
|
|
}
|
|
|
|
static TMatrix3x3 Scale2D(TVector2<vec_t> scaleVec)
|
|
{
|
|
TMatrix3x3 ret;
|
|
ret.Cells[0][0] = scaleVec.X; ret.Cells[0][1] = 0; ret.Cells[0][2] = 0;
|
|
ret.Cells[1][0] = 0; ret.Cells[1][1] = scaleVec.Y; ret.Cells[1][2] = 0;
|
|
ret.Cells[2][0] = 0; ret.Cells[2][1] = 0; ret.Cells[2][2] = 1;
|
|
return ret;
|
|
}
|
|
|
|
static TMatrix3x3 Translate2D(TVector2<vec_t> translateVec)
|
|
{
|
|
TMatrix3x3 ret;
|
|
ret.Cells[0][0] = 1; ret.Cells[0][1] = 0; ret.Cells[0][2] = translateVec.X;
|
|
ret.Cells[1][0] = 0; ret.Cells[1][1] = 1; ret.Cells[1][2] = translateVec.Y;
|
|
ret.Cells[2][0] = 0; ret.Cells[2][1] = 0; ret.Cells[2][2] = 1;
|
|
return ret;
|
|
}
|
|
|
|
void Zero()
|
|
{
|
|
memset (this, 0, sizeof *this);
|
|
}
|
|
|
|
void Identity()
|
|
{
|
|
Cells[0][0] = 1; Cells[0][1] = 0; Cells[0][2] = 0;
|
|
Cells[1][0] = 0; Cells[1][1] = 1; Cells[1][2] = 0;
|
|
Cells[2][0] = 0; Cells[2][1] = 0; Cells[2][2] = 1;
|
|
}
|
|
|
|
Vector3 &operator[] (int index)
|
|
{
|
|
return *((Vector3 *)&Cells[index]);
|
|
}
|
|
|
|
const Vector3 &operator[] (int index) const
|
|
{
|
|
return *((Vector3 *)&Cells[index]);
|
|
}
|
|
|
|
// Multiply a scalar
|
|
TMatrix3x3 &operator*= (double scalar)
|
|
{
|
|
(*this)[0] *= scalar;
|
|
(*this)[1] *= scalar;
|
|
(*this)[2] *= scalar;
|
|
return *this;
|
|
}
|
|
|
|
friend TMatrix3x3 operator* (double s, const TMatrix3x3 &m)
|
|
{
|
|
return TMatrix3x3(m[0]*s, m[1]*s, m[2]*s);
|
|
}
|
|
|
|
TMatrix3x3 operator* (double s) const
|
|
{
|
|
return TMatrix3x3((*this)[0]*s, (*this)[1]*s, (*this)[2]*s);
|
|
}
|
|
|
|
// Divide a scalar
|
|
TMatrix3x3 &operator/= (double scalar)
|
|
{
|
|
return *this *= 1 / scalar;
|
|
}
|
|
|
|
TMatrix3x3 operator/ (double s) const
|
|
{
|
|
return *this * (1 / s);
|
|
}
|
|
|
|
// Add two 3x3 matrices together
|
|
TMatrix3x3 &operator+= (const TMatrix3x3 &o)
|
|
{
|
|
(*this)[0] += o[0];
|
|
(*this)[1] += o[1];
|
|
(*this)[2] += o[2];
|
|
return *this;
|
|
}
|
|
|
|
TMatrix3x3 operator+ (const TMatrix3x3 &o) const
|
|
{
|
|
return TMatrix3x3((*this)[0] + o[0], (*this)[1] + o[1], (*this)[2] + o[2]);
|
|
}
|
|
|
|
// Subtract two 3x3 matrices
|
|
TMatrix3x3 &operator-= (const TMatrix3x3 &o)
|
|
{
|
|
(*this)[0] -= o[0];
|
|
(*this)[1] -= o[1];
|
|
(*this)[2] -= o[2];
|
|
return *this;
|
|
}
|
|
|
|
TMatrix3x3 operator- (const TMatrix3x3 &o) const
|
|
{
|
|
return TMatrix3x3((*this)[0] - o[0], (*this)[1] - o[1], (*this)[2] - o[2]);
|
|
}
|
|
|
|
// Concatenate two 3x3 matrices
|
|
TMatrix3x3 &operator*= (const TMatrix3x3 &o)
|
|
{
|
|
return *this = *this * o;
|
|
}
|
|
|
|
TMatrix3x3 operator* (const TMatrix3x3 &o) const
|
|
{
|
|
return TMatrix3x3(
|
|
Vector3(Cells[0][0]*o[0][0] + Cells[0][1]*o[1][0] + Cells[0][2]*o[2][0],
|
|
Cells[0][0]*o[0][1] + Cells[0][1]*o[1][1] + Cells[0][2]*o[2][1],
|
|
Cells[0][0]*o[0][2] + Cells[0][1]*o[1][2] + Cells[0][2]*o[2][2]),
|
|
Vector3(Cells[1][0]*o[0][0] + Cells[1][1]*o[1][0] + Cells[1][2]*o[2][0],
|
|
Cells[1][0]*o[0][1] + Cells[1][1]*o[1][1] + Cells[1][2]*o[2][1],
|
|
Cells[1][0]*o[0][2] + Cells[1][1]*o[1][2] + Cells[1][2]*o[2][2]),
|
|
Vector3(Cells[2][0]*o[0][0] + Cells[2][1]*o[1][0] + Cells[2][2]*o[2][0],
|
|
Cells[2][0]*o[0][1] + Cells[2][1]*o[1][1] + Cells[2][2]*o[2][1],
|
|
Cells[2][0]*o[0][2] + Cells[2][1]*o[1][2] + Cells[2][2]*o[2][2]));
|
|
}
|
|
|
|
// Multiply a 3D vector by a rotation matrix
|
|
friend Vector3 operator* (const Vector3 &v, const TMatrix3x3 &m)
|
|
{
|
|
return Vector3(m[0] | v, m[1] | v, m[2] | v);
|
|
}
|
|
|
|
friend Vector3 operator* (const TMatrix3x3 &m, const Vector3 &v)
|
|
{
|
|
return Vector3(m[0] | v, m[1] | v, m[2] | v);
|
|
}
|
|
};
|
|
|
|
#define BAM_FACTOR (90. / 0x40000000)
|
|
|
|
template<class vec_t>
|
|
struct TAngle
|
|
{
|
|
vec_t Degrees_;
|
|
|
|
// This is to catch any accidental attempt to assign an angle_t to this type. Any explicit exception will require a type cast.
|
|
TAngle(int) = delete;
|
|
TAngle(unsigned int) = delete;
|
|
TAngle(long) = delete;
|
|
TAngle(unsigned long) = delete;
|
|
TAngle &operator= (int other) = delete;
|
|
TAngle &operator= (unsigned other) = delete;
|
|
TAngle &operator= (long other) = delete;
|
|
TAngle &operator= (unsigned long other) = delete;
|
|
|
|
TAngle() = default;
|
|
|
|
private:
|
|
// Both constructors are needed to avoid unnecessary conversions when assigning to FAngle.
|
|
constexpr TAngle (float amt)
|
|
: Degrees_((vec_t)amt)
|
|
{
|
|
}
|
|
constexpr TAngle (double amt)
|
|
: Degrees_((vec_t)amt)
|
|
{
|
|
}
|
|
public:
|
|
|
|
vec_t& Degrees__() { return Degrees_; }
|
|
|
|
static constexpr TAngle fromDeg(float deg)
|
|
{
|
|
return TAngle(deg);
|
|
}
|
|
static constexpr TAngle fromDeg(double deg)
|
|
{
|
|
return TAngle(deg);
|
|
}
|
|
static constexpr TAngle fromDeg(int deg)
|
|
{
|
|
return TAngle((vec_t)deg);
|
|
}
|
|
static constexpr TAngle fromDeg(unsigned deg)
|
|
{
|
|
return TAngle((vec_t)deg);
|
|
}
|
|
|
|
static constexpr TAngle fromRad(float rad)
|
|
{
|
|
return TAngle(float(rad * (180.0f / pi::pif())));
|
|
}
|
|
static constexpr TAngle fromRad(double rad)
|
|
{
|
|
return TAngle(double(rad * (180.0 / pi::pi())));
|
|
}
|
|
|
|
static constexpr TAngle fromBam(int f)
|
|
{
|
|
return TAngle(f * (90. / 0x40000000));
|
|
}
|
|
static constexpr TAngle fromBam(unsigned f)
|
|
{
|
|
return TAngle(f * (90. / 0x40000000));
|
|
}
|
|
|
|
static constexpr TAngle fromBuild(int bang)
|
|
{
|
|
return TAngle(bang * (90. / 512));
|
|
}
|
|
|
|
static constexpr TAngle fromBuildf(double bang)
|
|
{
|
|
return TAngle(bang * (90. / 512));
|
|
}
|
|
|
|
static constexpr TAngle fromQ16(int bang)
|
|
{
|
|
return TAngle(bang * (90. / 16384));
|
|
}
|
|
|
|
TAngle(const TAngle &other) = default;
|
|
TAngle &operator= (const TAngle &other) = default;
|
|
|
|
constexpr TAngle operator- () const
|
|
{
|
|
return TAngle(-Degrees_);
|
|
}
|
|
|
|
constexpr TAngle &operator+= (TAngle other)
|
|
{
|
|
Degrees_ += other.Degrees_;
|
|
return *this;
|
|
}
|
|
|
|
constexpr TAngle &operator-= (TAngle other)
|
|
{
|
|
Degrees_ -= other.Degrees_;
|
|
return *this;
|
|
}
|
|
|
|
constexpr TAngle operator+ (TAngle other) const
|
|
{
|
|
return Degrees_ + other.Degrees_;
|
|
}
|
|
|
|
constexpr TAngle operator- (TAngle other) const
|
|
{
|
|
return Degrees_ - other.Degrees_;
|
|
}
|
|
|
|
constexpr TAngle &operator*= (vec_t other)
|
|
{
|
|
Degrees_ = Degrees_ * other;
|
|
return *this;
|
|
}
|
|
|
|
constexpr TAngle &operator/= (vec_t other)
|
|
{
|
|
Degrees_ = Degrees_ / other;
|
|
return *this;
|
|
}
|
|
|
|
constexpr TAngle operator* (vec_t other) const
|
|
{
|
|
return Degrees_ * other;
|
|
}
|
|
|
|
constexpr TAngle operator/ (vec_t other) const
|
|
{
|
|
return Degrees_ / other;
|
|
}
|
|
|
|
// Should the comparisons consider an epsilon value?
|
|
constexpr bool operator< (TAngle other) const
|
|
{
|
|
return Degrees_ < other.Degrees_;
|
|
}
|
|
|
|
constexpr bool operator> (TAngle other) const
|
|
{
|
|
return Degrees_ > other.Degrees_;
|
|
}
|
|
|
|
constexpr bool operator<= (TAngle other) const
|
|
{
|
|
return Degrees_ <= other.Degrees_;
|
|
}
|
|
|
|
constexpr bool operator>= (TAngle other) const
|
|
{
|
|
return Degrees_ >= other.Degrees_;
|
|
}
|
|
|
|
constexpr bool operator== (TAngle other) const
|
|
{
|
|
return Degrees_ == other.Degrees_;
|
|
}
|
|
|
|
constexpr bool operator!= (TAngle other) const
|
|
{
|
|
return Degrees_ != other.Degrees_;
|
|
}
|
|
|
|
// Ensure the angle is between [0.0,360.0) degrees
|
|
TAngle Normalized360() const
|
|
{
|
|
// Normalizing the angle converts it to a BAM, which masks it, and converts it back to a float.
|
|
// Note: We MUST use xs_Float here because it is the only method that guarantees reliable wraparound.
|
|
return (vec_t)(BAM_FACTOR * BAMs());
|
|
}
|
|
|
|
// Ensures the angle is between (-180.0,180.0] degrees
|
|
TAngle Normalized180() const
|
|
{
|
|
return (vec_t)(BAM_FACTOR * (signed int)BAMs());
|
|
}
|
|
|
|
constexpr vec_t Radians() const
|
|
{
|
|
return vec_t(Degrees_ * (pi::pi() / 180.0));
|
|
}
|
|
|
|
unsigned BAMs() const
|
|
{
|
|
return xs_CRoundToInt(Degrees_ * (0x40000000 / 90.));
|
|
}
|
|
|
|
constexpr vec_t Degrees() const
|
|
{
|
|
return Degrees_;
|
|
}
|
|
|
|
constexpr int Buildang() const
|
|
{
|
|
return int(Degrees_ * (512 / 90.0));
|
|
}
|
|
|
|
constexpr double Buildfang() const
|
|
{
|
|
return Degrees_ * (512 / 90.0);
|
|
}
|
|
|
|
constexpr int Q16() const
|
|
{
|
|
return int(Degrees_ * (16384 / 90.0));
|
|
}
|
|
|
|
TVector2<vec_t> ToVector(vec_t length = 1) const
|
|
{
|
|
return TVector2<vec_t>(length * Cos(), length * Sin());
|
|
}
|
|
|
|
vec_t Cos() const
|
|
{
|
|
return vec_t(g_cosdeg(Degrees_));
|
|
}
|
|
|
|
vec_t Sin() const
|
|
{
|
|
return vec_t(g_sindeg(Degrees_));
|
|
}
|
|
|
|
double Tan() const
|
|
{
|
|
return vec_t(g_tan(Radians()));
|
|
}
|
|
|
|
// This is for calculating vertical velocity. For high pitches the tangent will become too large to be useful.
|
|
double TanClamped(double max = 5.) const
|
|
{
|
|
return clamp(Tan(), -max, max);
|
|
}
|
|
|
|
constexpr int Sgn() const
|
|
{
|
|
const auto normalized = (signed int)BAMs();
|
|
return (normalized > 0) - (normalized < 0);
|
|
}
|
|
};
|
|
|
|
template<class T>
|
|
inline TAngle<T> fabs (const TAngle<T> °)
|
|
{
|
|
return TAngle<T>::fromDeg(fabs(deg.Degrees()));
|
|
}
|
|
|
|
template<class T>
|
|
inline TAngle<T> abs (const TAngle<T> °)
|
|
{
|
|
return TAngle<T>::fromDeg(fabs(deg.Degrees()));
|
|
}
|
|
|
|
template<class T>
|
|
inline TAngle<T> deltaangle(const TAngle<T> &a1, const TAngle<T> &a2)
|
|
{
|
|
return (a2 - a1).Normalized180();
|
|
}
|
|
|
|
template<class T>
|
|
inline TAngle<T> absangle(const TAngle<T> &a1, const TAngle<T> &a2)
|
|
{
|
|
return fabs((a1 - a2).Normalized180());
|
|
}
|
|
|
|
inline TAngle<double> VecToAngle(double x, double y)
|
|
{
|
|
return TAngle<double>::fromRad(g_atan2(y, x));
|
|
}
|
|
|
|
template<class T>
|
|
inline TAngle<T> VecToAngle (const TVector2<T> &vec)
|
|
{
|
|
return TAngle<T>::fromRad(g_atan2(vec.Y, vec.X));
|
|
}
|
|
|
|
template<class T>
|
|
inline TAngle<T> VecToAngle (const TVector3<T> &vec)
|
|
{
|
|
return TAngle<T>::fromRad(g_atan2(vec.Y, vec.X));
|
|
}
|
|
|
|
template<class T>
|
|
TAngle<T> TVector2<T>::Angle() const
|
|
{
|
|
return VecToAngle(X, Y);
|
|
}
|
|
|
|
template<class T>
|
|
TAngle<T> TVector3<T>::Angle() const
|
|
{
|
|
return VecToAngle(X, Y);
|
|
}
|
|
|
|
template<class T>
|
|
TAngle<T> TVector3<T>::Pitch() const
|
|
{
|
|
return -VecToAngle(TVector2<T>(X, Y).Length(), Z);
|
|
}
|
|
|
|
// Much of this is copied from TVector3. Is all that functionality really appropriate?
|
|
template<class vec_t>
|
|
struct TRotator
|
|
{
|
|
typedef TAngle<vec_t> Angle;
|
|
|
|
Angle Pitch; // up/down
|
|
Angle Yaw; // left/right
|
|
Angle Roll; // rotation about the forward axis.
|
|
Angle CamRoll; // Roll specific to actor cameras. Used by quakes.
|
|
|
|
TRotator() = default;
|
|
|
|
TRotator (const Angle &p, const Angle &y, const Angle &r)
|
|
: Pitch(p), Yaw(y), Roll(r)
|
|
{
|
|
}
|
|
|
|
TRotator(const TRotator &other) = default;
|
|
TRotator &operator= (const TRotator &other) = default;
|
|
|
|
// Access angles as an array
|
|
Angle &operator[] (int index)
|
|
{
|
|
return *(&Pitch + index);
|
|
}
|
|
|
|
const Angle &operator[] (int index) const
|
|
{
|
|
return *(&Pitch + index);
|
|
}
|
|
|
|
// Test for equality
|
|
bool operator== (const TRotator &other) const
|
|
{
|
|
return fabs(Pitch - other.Pitch) < Angle(EQUAL_EPSILON) && fabs(Yaw - other.Yaw) < Angle(EQUAL_EPSILON) && fabs(Roll - other.Roll) < Angle(EQUAL_EPSILON);
|
|
}
|
|
|
|
// Test for inequality
|
|
bool operator!= (const TRotator &other) const
|
|
{
|
|
return fabs(Pitch - other.Pitch) >= Angle(EQUAL_EPSILON) && fabs(Yaw - other.Yaw) >= Angle(EQUAL_EPSILON) && fabs(Roll - other.Roll) >= Angle(EQUAL_EPSILON);
|
|
}
|
|
|
|
// Unary negation
|
|
TRotator operator- () const
|
|
{
|
|
return TRotator(-Pitch, -Yaw, -Roll);
|
|
}
|
|
|
|
// Scalar addition
|
|
TRotator &operator+= (const Angle &scalar)
|
|
{
|
|
Pitch += scalar, Yaw += scalar, Roll += scalar;
|
|
return *this;
|
|
}
|
|
|
|
friend TRotator operator+ (const TRotator &v, const Angle &scalar)
|
|
{
|
|
return TRotator(v.Pitch + scalar, v.Yaw + scalar, v.Roll + scalar);
|
|
}
|
|
|
|
friend TRotator operator+ (const Angle &scalar, const TRotator &v)
|
|
{
|
|
return TRotator(v.Pitch + scalar, v.Yaw + scalar, v.Roll + scalar);
|
|
}
|
|
|
|
// Scalar subtraction
|
|
TRotator &operator-= (const Angle &scalar)
|
|
{
|
|
Pitch -= scalar, Yaw -= scalar, Roll -= scalar;
|
|
return *this;
|
|
}
|
|
|
|
TRotator operator- (const Angle &scalar) const
|
|
{
|
|
return TRotator(Pitch - scalar, Yaw - scalar, Roll - scalar);
|
|
}
|
|
|
|
// Scalar multiplication
|
|
TRotator &operator*= (const Angle &scalar)
|
|
{
|
|
Pitch *= scalar, Yaw *= scalar, Roll *= scalar;
|
|
return *this;
|
|
}
|
|
|
|
friend TRotator operator* (const TRotator &v, const Angle &scalar)
|
|
{
|
|
return TRotator(v.Pitch * scalar, v.Yaw * scalar, v.Roll * scalar);
|
|
}
|
|
|
|
friend TRotator operator* (const Angle &scalar, const TRotator &v)
|
|
{
|
|
return TRotator(v.Pitch * scalar, v.Yaw * scalar, v.Roll * scalar);
|
|
}
|
|
|
|
// Scalar division
|
|
TRotator &operator/= (const Angle &scalar)
|
|
{
|
|
Angle mul(1 / scalar.Degrees_);
|
|
Pitch *= scalar, Yaw *= scalar, Roll *= scalar;
|
|
return *this;
|
|
}
|
|
|
|
TRotator operator/ (const Angle &scalar) const
|
|
{
|
|
Angle mul(1 / scalar.Degrees_);
|
|
return TRotator(Pitch * mul, Yaw * mul, Roll * mul);
|
|
}
|
|
|
|
// Vector addition
|
|
TRotator &operator+= (const TRotator &other)
|
|
{
|
|
Pitch += other.Pitch, Yaw += other.Yaw, Roll += other.Roll;
|
|
return *this;
|
|
}
|
|
|
|
TRotator operator+ (const TRotator &other) const
|
|
{
|
|
return TRotator(Pitch + other.Pitch, Yaw + other.Yaw, Roll + other.Roll);
|
|
}
|
|
|
|
// Vector subtraction
|
|
TRotator &operator-= (const TRotator &other)
|
|
{
|
|
Pitch -= other.Pitch, Yaw -= other.Yaw, Roll -= other.Roll;
|
|
return *this;
|
|
}
|
|
|
|
TRotator operator- (const TRotator &other) const
|
|
{
|
|
return TRotator(Pitch - other.Pitch, Yaw - other.Yaw, Roll - other.Roll);
|
|
}
|
|
};
|
|
|
|
// Create a forward vector from a rotation (ignoring roll)
|
|
|
|
template<class T>
|
|
inline TVector3<T>::TVector3 (const TRotator<T> &rot)
|
|
{
|
|
double pcos = rot.Pitch.Cos();
|
|
X = pcos * rot.Yaw.Cos();
|
|
Y = pcos * rot.Yaw.Sin();
|
|
Z = rot.Pitch.Sin();
|
|
}
|
|
|
|
template<class T>
|
|
inline TMatrix3x3<T>::TMatrix3x3(const TVector3<T> &axis, TAngle<T> degrees)
|
|
{
|
|
double c = degrees.Cos(), s = degrees.Sin(), t = 1 - c;
|
|
double sx = s*axis.X, sy = s*axis.Y, sz = s*axis.Z;
|
|
double tx, ty, txx, tyy, u, v;
|
|
|
|
tx = t*axis.X;
|
|
Cells[0][0] = T( (txx=tx*axis.X) + c );
|
|
Cells[0][1] = T( (u=tx*axis.Y) - sz );
|
|
Cells[0][2] = T( (v=tx*axis.Z) + sy );
|
|
|
|
ty = t*axis.Y;
|
|
Cells[1][0] = T( u + sz );
|
|
Cells[1][1] = T( (tyy=ty*axis.Y) + c );
|
|
Cells[1][2] = T( (u=ty*axis.Z) - sx );
|
|
|
|
Cells[2][0] = T( v - sy );
|
|
Cells[2][1] = T( u + sx );
|
|
Cells[2][2] = T( (t-txx-tyy) + c );
|
|
}
|
|
|
|
|
|
typedef TVector2<float> FVector2;
|
|
typedef TVector3<float> FVector3;
|
|
typedef TVector4<float> FVector4;
|
|
typedef TRotator<float> FRotator;
|
|
typedef TMatrix3x3<float> FMatrix3x3;
|
|
typedef TAngle<float> FAngle;
|
|
|
|
typedef TVector2<double> DVector2;
|
|
typedef TVector3<double> DVector3;
|
|
typedef TVector4<double> DVector4;
|
|
typedef TRotator<double> DRotator;
|
|
typedef TMatrix3x3<double> DMatrix3x3;
|
|
typedef TAngle<double> DAngle;
|
|
|
|
constexpr DAngle nullAngle = DAngle::fromDeg(0.);
|
|
constexpr FAngle nullFAngle = FAngle::fromDeg(0.);
|
|
|
|
class Plane
|
|
{
|
|
public:
|
|
void Set(FVector3 normal, float d)
|
|
{
|
|
m_normal = normal;
|
|
m_d = d;
|
|
}
|
|
|
|
void Init(const FVector3& p1, const FVector3& p2, const FVector3& p3)
|
|
{
|
|
m_normal = ((p2 - p1) ^ (p3 - p1)).Unit();
|
|
m_d = -(p3 |m_normal);
|
|
}
|
|
|
|
// same for a play-vector. Note that y and z are inversed.
|
|
void Set(DVector3 normal, double d)
|
|
{
|
|
m_normal = { (float)normal.X, (float)normal.Z, (float)normal.Y };
|
|
m_d = (float)d;
|
|
}
|
|
|
|
float DistToPoint(float x, float y, float z)
|
|
{
|
|
FVector3 p(x, y, z);
|
|
|
|
return (m_normal | p) + m_d;
|
|
}
|
|
|
|
|
|
bool PointOnSide(float x, float y, float z)
|
|
{
|
|
return DistToPoint(x, y, z) < 0.f;
|
|
}
|
|
|
|
bool PointOnSide(const FVector3 &v) { return PointOnSide(v.X, v.Y, v.Z); }
|
|
|
|
float A() { return m_normal.X; }
|
|
float B() { return m_normal.Y; }
|
|
float C() { return m_normal.Z; }
|
|
float D() { return m_d; }
|
|
|
|
const FVector3 &Normal() const { return m_normal; }
|
|
protected:
|
|
FVector3 m_normal;
|
|
float m_d;
|
|
};
|
|
|
|
|
|
#endif
|