mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 05:40:05 +00:00
ab8f16961e
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files. make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type. This commit contains no other changes aside from adding and applying clang-format-all.sh
1420 lines
27 KiB
C++
1420 lines
27 KiB
C++
/*
|
|
Copyright (C) 2009 Sony Computer Entertainment Inc.
|
|
All rights reserved.
|
|
|
|
This software is provided 'as-is', without any express or implied warranty.
|
|
In no event will the authors be held liable for any damages arising from the use of this software.
|
|
Permission is granted to anyone to use this software for any purpose,
|
|
including commercial applications, and to alter it and redistribute it freely,
|
|
subject to the following restrictions:
|
|
|
|
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
|
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
|
3. This notice may not be removed or altered from any source distribution.
|
|
|
|
*/
|
|
|
|
#ifndef _VECTORMATH_VEC_AOS_CPP_H
|
|
#define _VECTORMATH_VEC_AOS_CPP_H
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Constants
|
|
|
|
#define _VECTORMATH_SLERP_TOL 0.999f
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Definitions
|
|
|
|
#ifndef _VECTORMATH_INTERNAL_FUNCTIONS
|
|
#define _VECTORMATH_INTERNAL_FUNCTIONS
|
|
|
|
#endif
|
|
|
|
namespace Vectormath
|
|
{
|
|
namespace Aos
|
|
{
|
|
inline Vector3::Vector3(const Vector3 &vec)
|
|
{
|
|
mX = vec.mX;
|
|
mY = vec.mY;
|
|
mZ = vec.mZ;
|
|
}
|
|
|
|
inline Vector3::Vector3(float _x, float _y, float _z)
|
|
{
|
|
mX = _x;
|
|
mY = _y;
|
|
mZ = _z;
|
|
}
|
|
|
|
inline Vector3::Vector3(const Point3 &pnt)
|
|
{
|
|
mX = pnt.getX();
|
|
mY = pnt.getY();
|
|
mZ = pnt.getZ();
|
|
}
|
|
|
|
inline Vector3::Vector3(float scalar)
|
|
{
|
|
mX = scalar;
|
|
mY = scalar;
|
|
mZ = scalar;
|
|
}
|
|
|
|
inline const Vector3 Vector3::xAxis()
|
|
{
|
|
return Vector3(1.0f, 0.0f, 0.0f);
|
|
}
|
|
|
|
inline const Vector3 Vector3::yAxis()
|
|
{
|
|
return Vector3(0.0f, 1.0f, 0.0f);
|
|
}
|
|
|
|
inline const Vector3 Vector3::zAxis()
|
|
{
|
|
return Vector3(0.0f, 0.0f, 1.0f);
|
|
}
|
|
|
|
inline const Vector3 lerp(float t, const Vector3 &vec0, const Vector3 &vec1)
|
|
{
|
|
return (vec0 + ((vec1 - vec0) * t));
|
|
}
|
|
|
|
inline const Vector3 slerp(float t, const Vector3 &unitVec0, const Vector3 &unitVec1)
|
|
{
|
|
float recipSinAngle, scale0, scale1, cosAngle, angle;
|
|
cosAngle = dot(unitVec0, unitVec1);
|
|
if (cosAngle < _VECTORMATH_SLERP_TOL)
|
|
{
|
|
angle = acosf(cosAngle);
|
|
recipSinAngle = (1.0f / sinf(angle));
|
|
scale0 = (sinf(((1.0f - t) * angle)) * recipSinAngle);
|
|
scale1 = (sinf((t * angle)) * recipSinAngle);
|
|
}
|
|
else
|
|
{
|
|
scale0 = (1.0f - t);
|
|
scale1 = t;
|
|
}
|
|
return ((unitVec0 * scale0) + (unitVec1 * scale1));
|
|
}
|
|
|
|
inline void loadXYZ(Vector3 &vec, const float *fptr)
|
|
{
|
|
vec = Vector3(fptr[0], fptr[1], fptr[2]);
|
|
}
|
|
|
|
inline void storeXYZ(const Vector3 &vec, float *fptr)
|
|
{
|
|
fptr[0] = vec.getX();
|
|
fptr[1] = vec.getY();
|
|
fptr[2] = vec.getZ();
|
|
}
|
|
|
|
inline void loadHalfFloats(Vector3 &vec, const unsigned short *hfptr)
|
|
{
|
|
union Data32 {
|
|
unsigned int u32;
|
|
float f32;
|
|
};
|
|
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
unsigned short fp16 = hfptr[i];
|
|
unsigned int sign = fp16 >> 15;
|
|
unsigned int exponent = (fp16 >> 10) & ((1 << 5) - 1);
|
|
unsigned int mantissa = fp16 & ((1 << 10) - 1);
|
|
|
|
if (exponent == 0)
|
|
{
|
|
// zero
|
|
mantissa = 0;
|
|
}
|
|
else if (exponent == 31)
|
|
{
|
|
// infinity or nan -> infinity
|
|
exponent = 255;
|
|
mantissa = 0;
|
|
}
|
|
else
|
|
{
|
|
exponent += 127 - 15;
|
|
mantissa <<= 13;
|
|
}
|
|
|
|
Data32 d;
|
|
d.u32 = (sign << 31) | (exponent << 23) | mantissa;
|
|
vec[i] = d.f32;
|
|
}
|
|
}
|
|
|
|
inline void storeHalfFloats(const Vector3 &vec, unsigned short *hfptr)
|
|
{
|
|
union Data32 {
|
|
unsigned int u32;
|
|
float f32;
|
|
};
|
|
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
Data32 d;
|
|
d.f32 = vec[i];
|
|
|
|
unsigned int sign = d.u32 >> 31;
|
|
unsigned int exponent = (d.u32 >> 23) & ((1 << 8) - 1);
|
|
unsigned int mantissa = d.u32 & ((1 << 23) - 1);
|
|
;
|
|
|
|
if (exponent == 0)
|
|
{
|
|
// zero or denorm -> zero
|
|
mantissa = 0;
|
|
}
|
|
else if (exponent == 255 && mantissa != 0)
|
|
{
|
|
// nan -> infinity
|
|
exponent = 31;
|
|
mantissa = 0;
|
|
}
|
|
else if (exponent >= 127 - 15 + 31)
|
|
{
|
|
// overflow or infinity -> infinity
|
|
exponent = 31;
|
|
mantissa = 0;
|
|
}
|
|
else if (exponent <= 127 - 15)
|
|
{
|
|
// underflow -> zero
|
|
exponent = 0;
|
|
mantissa = 0;
|
|
}
|
|
else
|
|
{
|
|
exponent -= 127 - 15;
|
|
mantissa >>= 13;
|
|
}
|
|
|
|
hfptr[i] = (unsigned short)((sign << 15) | (exponent << 10) | mantissa);
|
|
}
|
|
}
|
|
|
|
inline Vector3 &Vector3::operator=(const Vector3 &vec)
|
|
{
|
|
mX = vec.mX;
|
|
mY = vec.mY;
|
|
mZ = vec.mZ;
|
|
return *this;
|
|
}
|
|
|
|
inline Vector3 &Vector3::setX(float _x)
|
|
{
|
|
mX = _x;
|
|
return *this;
|
|
}
|
|
|
|
inline float Vector3::getX() const
|
|
{
|
|
return mX;
|
|
}
|
|
|
|
inline Vector3 &Vector3::setY(float _y)
|
|
{
|
|
mY = _y;
|
|
return *this;
|
|
}
|
|
|
|
inline float Vector3::getY() const
|
|
{
|
|
return mY;
|
|
}
|
|
|
|
inline Vector3 &Vector3::setZ(float _z)
|
|
{
|
|
mZ = _z;
|
|
return *this;
|
|
}
|
|
|
|
inline float Vector3::getZ() const
|
|
{
|
|
return mZ;
|
|
}
|
|
|
|
inline Vector3 &Vector3::setElem(int idx, float value)
|
|
{
|
|
*(&mX + idx) = value;
|
|
return *this;
|
|
}
|
|
|
|
inline float Vector3::getElem(int idx) const
|
|
{
|
|
return *(&mX + idx);
|
|
}
|
|
|
|
inline float &Vector3::operator[](int idx)
|
|
{
|
|
return *(&mX + idx);
|
|
}
|
|
|
|
inline float Vector3::operator[](int idx) const
|
|
{
|
|
return *(&mX + idx);
|
|
}
|
|
|
|
inline const Vector3 Vector3::operator+(const Vector3 &vec) const
|
|
{
|
|
return Vector3(
|
|
(mX + vec.mX),
|
|
(mY + vec.mY),
|
|
(mZ + vec.mZ));
|
|
}
|
|
|
|
inline const Vector3 Vector3::operator-(const Vector3 &vec) const
|
|
{
|
|
return Vector3(
|
|
(mX - vec.mX),
|
|
(mY - vec.mY),
|
|
(mZ - vec.mZ));
|
|
}
|
|
|
|
inline const Point3 Vector3::operator+(const Point3 &pnt) const
|
|
{
|
|
return Point3(
|
|
(mX + pnt.getX()),
|
|
(mY + pnt.getY()),
|
|
(mZ + pnt.getZ()));
|
|
}
|
|
|
|
inline const Vector3 Vector3::operator*(float scalar) const
|
|
{
|
|
return Vector3(
|
|
(mX * scalar),
|
|
(mY * scalar),
|
|
(mZ * scalar));
|
|
}
|
|
|
|
inline Vector3 &Vector3::operator+=(const Vector3 &vec)
|
|
{
|
|
*this = *this + vec;
|
|
return *this;
|
|
}
|
|
|
|
inline Vector3 &Vector3::operator-=(const Vector3 &vec)
|
|
{
|
|
*this = *this - vec;
|
|
return *this;
|
|
}
|
|
|
|
inline Vector3 &Vector3::operator*=(float scalar)
|
|
{
|
|
*this = *this * scalar;
|
|
return *this;
|
|
}
|
|
|
|
inline const Vector3 Vector3::operator/(float scalar) const
|
|
{
|
|
return Vector3(
|
|
(mX / scalar),
|
|
(mY / scalar),
|
|
(mZ / scalar));
|
|
}
|
|
|
|
inline Vector3 &Vector3::operator/=(float scalar)
|
|
{
|
|
*this = *this / scalar;
|
|
return *this;
|
|
}
|
|
|
|
inline const Vector3 Vector3::operator-() const
|
|
{
|
|
return Vector3(
|
|
-mX,
|
|
-mY,
|
|
-mZ);
|
|
}
|
|
|
|
inline const Vector3 operator*(float scalar, const Vector3 &vec)
|
|
{
|
|
return vec * scalar;
|
|
}
|
|
|
|
inline const Vector3 mulPerElem(const Vector3 &vec0, const Vector3 &vec1)
|
|
{
|
|
return Vector3(
|
|
(vec0.getX() * vec1.getX()),
|
|
(vec0.getY() * vec1.getY()),
|
|
(vec0.getZ() * vec1.getZ()));
|
|
}
|
|
|
|
inline const Vector3 divPerElem(const Vector3 &vec0, const Vector3 &vec1)
|
|
{
|
|
return Vector3(
|
|
(vec0.getX() / vec1.getX()),
|
|
(vec0.getY() / vec1.getY()),
|
|
(vec0.getZ() / vec1.getZ()));
|
|
}
|
|
|
|
inline const Vector3 recipPerElem(const Vector3 &vec)
|
|
{
|
|
return Vector3(
|
|
(1.0f / vec.getX()),
|
|
(1.0f / vec.getY()),
|
|
(1.0f / vec.getZ()));
|
|
}
|
|
|
|
inline const Vector3 sqrtPerElem(const Vector3 &vec)
|
|
{
|
|
return Vector3(
|
|
sqrtf(vec.getX()),
|
|
sqrtf(vec.getY()),
|
|
sqrtf(vec.getZ()));
|
|
}
|
|
|
|
inline const Vector3 rsqrtPerElem(const Vector3 &vec)
|
|
{
|
|
return Vector3(
|
|
(1.0f / sqrtf(vec.getX())),
|
|
(1.0f / sqrtf(vec.getY())),
|
|
(1.0f / sqrtf(vec.getZ())));
|
|
}
|
|
|
|
inline const Vector3 absPerElem(const Vector3 &vec)
|
|
{
|
|
return Vector3(
|
|
fabsf(vec.getX()),
|
|
fabsf(vec.getY()),
|
|
fabsf(vec.getZ()));
|
|
}
|
|
|
|
inline const Vector3 copySignPerElem(const Vector3 &vec0, const Vector3 &vec1)
|
|
{
|
|
return Vector3(
|
|
(vec1.getX() < 0.0f) ? -fabsf(vec0.getX()) : fabsf(vec0.getX()),
|
|
(vec1.getY() < 0.0f) ? -fabsf(vec0.getY()) : fabsf(vec0.getY()),
|
|
(vec1.getZ() < 0.0f) ? -fabsf(vec0.getZ()) : fabsf(vec0.getZ()));
|
|
}
|
|
|
|
inline const Vector3 maxPerElem(const Vector3 &vec0, const Vector3 &vec1)
|
|
{
|
|
return Vector3(
|
|
(vec0.getX() > vec1.getX()) ? vec0.getX() : vec1.getX(),
|
|
(vec0.getY() > vec1.getY()) ? vec0.getY() : vec1.getY(),
|
|
(vec0.getZ() > vec1.getZ()) ? vec0.getZ() : vec1.getZ());
|
|
}
|
|
|
|
inline float maxElem(const Vector3 &vec)
|
|
{
|
|
float result;
|
|
result = (vec.getX() > vec.getY()) ? vec.getX() : vec.getY();
|
|
result = (vec.getZ() > result) ? vec.getZ() : result;
|
|
return result;
|
|
}
|
|
|
|
inline const Vector3 minPerElem(const Vector3 &vec0, const Vector3 &vec1)
|
|
{
|
|
return Vector3(
|
|
(vec0.getX() < vec1.getX()) ? vec0.getX() : vec1.getX(),
|
|
(vec0.getY() < vec1.getY()) ? vec0.getY() : vec1.getY(),
|
|
(vec0.getZ() < vec1.getZ()) ? vec0.getZ() : vec1.getZ());
|
|
}
|
|
|
|
inline float minElem(const Vector3 &vec)
|
|
{
|
|
float result;
|
|
result = (vec.getX() < vec.getY()) ? vec.getX() : vec.getY();
|
|
result = (vec.getZ() < result) ? vec.getZ() : result;
|
|
return result;
|
|
}
|
|
|
|
inline float sum(const Vector3 &vec)
|
|
{
|
|
float result;
|
|
result = (vec.getX() + vec.getY());
|
|
result = (result + vec.getZ());
|
|
return result;
|
|
}
|
|
|
|
inline float dot(const Vector3 &vec0, const Vector3 &vec1)
|
|
{
|
|
float result;
|
|
result = (vec0.getX() * vec1.getX());
|
|
result = (result + (vec0.getY() * vec1.getY()));
|
|
result = (result + (vec0.getZ() * vec1.getZ()));
|
|
return result;
|
|
}
|
|
|
|
inline float lengthSqr(const Vector3 &vec)
|
|
{
|
|
float result;
|
|
result = (vec.getX() * vec.getX());
|
|
result = (result + (vec.getY() * vec.getY()));
|
|
result = (result + (vec.getZ() * vec.getZ()));
|
|
return result;
|
|
}
|
|
|
|
inline float length(const Vector3 &vec)
|
|
{
|
|
return ::sqrtf(lengthSqr(vec));
|
|
}
|
|
|
|
inline const Vector3 normalize(const Vector3 &vec)
|
|
{
|
|
float lenSqr, lenInv;
|
|
lenSqr = lengthSqr(vec);
|
|
lenInv = (1.0f / sqrtf(lenSqr));
|
|
return Vector3(
|
|
(vec.getX() * lenInv),
|
|
(vec.getY() * lenInv),
|
|
(vec.getZ() * lenInv));
|
|
}
|
|
|
|
inline const Vector3 cross(const Vector3 &vec0, const Vector3 &vec1)
|
|
{
|
|
return Vector3(
|
|
((vec0.getY() * vec1.getZ()) - (vec0.getZ() * vec1.getY())),
|
|
((vec0.getZ() * vec1.getX()) - (vec0.getX() * vec1.getZ())),
|
|
((vec0.getX() * vec1.getY()) - (vec0.getY() * vec1.getX())));
|
|
}
|
|
|
|
inline const Vector3 select(const Vector3 &vec0, const Vector3 &vec1, bool select1)
|
|
{
|
|
return Vector3(
|
|
(select1) ? vec1.getX() : vec0.getX(),
|
|
(select1) ? vec1.getY() : vec0.getY(),
|
|
(select1) ? vec1.getZ() : vec0.getZ());
|
|
}
|
|
|
|
#ifdef _VECTORMATH_DEBUG
|
|
|
|
inline void print(const Vector3 &vec)
|
|
{
|
|
printf("( %f %f %f )\n", vec.getX(), vec.getY(), vec.getZ());
|
|
}
|
|
|
|
inline void print(const Vector3 &vec, const char *name)
|
|
{
|
|
printf("%s: ( %f %f %f )\n", name, vec.getX(), vec.getY(), vec.getZ());
|
|
}
|
|
|
|
#endif
|
|
|
|
inline Vector4::Vector4(const Vector4 &vec)
|
|
{
|
|
mX = vec.mX;
|
|
mY = vec.mY;
|
|
mZ = vec.mZ;
|
|
mW = vec.mW;
|
|
}
|
|
|
|
inline Vector4::Vector4(float _x, float _y, float _z, float _w)
|
|
{
|
|
mX = _x;
|
|
mY = _y;
|
|
mZ = _z;
|
|
mW = _w;
|
|
}
|
|
|
|
inline Vector4::Vector4(const Vector3 &xyz, float _w)
|
|
{
|
|
this->setXYZ(xyz);
|
|
this->setW(_w);
|
|
}
|
|
|
|
inline Vector4::Vector4(const Vector3 &vec)
|
|
{
|
|
mX = vec.getX();
|
|
mY = vec.getY();
|
|
mZ = vec.getZ();
|
|
mW = 0.0f;
|
|
}
|
|
|
|
inline Vector4::Vector4(const Point3 &pnt)
|
|
{
|
|
mX = pnt.getX();
|
|
mY = pnt.getY();
|
|
mZ = pnt.getZ();
|
|
mW = 1.0f;
|
|
}
|
|
|
|
inline Vector4::Vector4(const Quat &quat)
|
|
{
|
|
mX = quat.getX();
|
|
mY = quat.getY();
|
|
mZ = quat.getZ();
|
|
mW = quat.getW();
|
|
}
|
|
|
|
inline Vector4::Vector4(float scalar)
|
|
{
|
|
mX = scalar;
|
|
mY = scalar;
|
|
mZ = scalar;
|
|
mW = scalar;
|
|
}
|
|
|
|
inline const Vector4 Vector4::xAxis()
|
|
{
|
|
return Vector4(1.0f, 0.0f, 0.0f, 0.0f);
|
|
}
|
|
|
|
inline const Vector4 Vector4::yAxis()
|
|
{
|
|
return Vector4(0.0f, 1.0f, 0.0f, 0.0f);
|
|
}
|
|
|
|
inline const Vector4 Vector4::zAxis()
|
|
{
|
|
return Vector4(0.0f, 0.0f, 1.0f, 0.0f);
|
|
}
|
|
|
|
inline const Vector4 Vector4::wAxis()
|
|
{
|
|
return Vector4(0.0f, 0.0f, 0.0f, 1.0f);
|
|
}
|
|
|
|
inline const Vector4 lerp(float t, const Vector4 &vec0, const Vector4 &vec1)
|
|
{
|
|
return (vec0 + ((vec1 - vec0) * t));
|
|
}
|
|
|
|
inline const Vector4 slerp(float t, const Vector4 &unitVec0, const Vector4 &unitVec1)
|
|
{
|
|
float recipSinAngle, scale0, scale1, cosAngle, angle;
|
|
cosAngle = dot(unitVec0, unitVec1);
|
|
if (cosAngle < _VECTORMATH_SLERP_TOL)
|
|
{
|
|
angle = acosf(cosAngle);
|
|
recipSinAngle = (1.0f / sinf(angle));
|
|
scale0 = (sinf(((1.0f - t) * angle)) * recipSinAngle);
|
|
scale1 = (sinf((t * angle)) * recipSinAngle);
|
|
}
|
|
else
|
|
{
|
|
scale0 = (1.0f - t);
|
|
scale1 = t;
|
|
}
|
|
return ((unitVec0 * scale0) + (unitVec1 * scale1));
|
|
}
|
|
|
|
inline void loadXYZW(Vector4 &vec, const float *fptr)
|
|
{
|
|
vec = Vector4(fptr[0], fptr[1], fptr[2], fptr[3]);
|
|
}
|
|
|
|
inline void storeXYZW(const Vector4 &vec, float *fptr)
|
|
{
|
|
fptr[0] = vec.getX();
|
|
fptr[1] = vec.getY();
|
|
fptr[2] = vec.getZ();
|
|
fptr[3] = vec.getW();
|
|
}
|
|
|
|
inline void loadHalfFloats(Vector4 &vec, const unsigned short *hfptr)
|
|
{
|
|
union Data32 {
|
|
unsigned int u32;
|
|
float f32;
|
|
};
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
unsigned short fp16 = hfptr[i];
|
|
unsigned int sign = fp16 >> 15;
|
|
unsigned int exponent = (fp16 >> 10) & ((1 << 5) - 1);
|
|
unsigned int mantissa = fp16 & ((1 << 10) - 1);
|
|
|
|
if (exponent == 0)
|
|
{
|
|
// zero
|
|
mantissa = 0;
|
|
}
|
|
else if (exponent == 31)
|
|
{
|
|
// infinity or nan -> infinity
|
|
exponent = 255;
|
|
mantissa = 0;
|
|
}
|
|
else
|
|
{
|
|
exponent += 127 - 15;
|
|
mantissa <<= 13;
|
|
}
|
|
|
|
Data32 d;
|
|
d.u32 = (sign << 31) | (exponent << 23) | mantissa;
|
|
vec[i] = d.f32;
|
|
}
|
|
}
|
|
|
|
inline void storeHalfFloats(const Vector4 &vec, unsigned short *hfptr)
|
|
{
|
|
union Data32 {
|
|
unsigned int u32;
|
|
float f32;
|
|
};
|
|
|
|
for (int i = 0; i < 4; i++)
|
|
{
|
|
Data32 d;
|
|
d.f32 = vec[i];
|
|
|
|
unsigned int sign = d.u32 >> 31;
|
|
unsigned int exponent = (d.u32 >> 23) & ((1 << 8) - 1);
|
|
unsigned int mantissa = d.u32 & ((1 << 23) - 1);
|
|
;
|
|
|
|
if (exponent == 0)
|
|
{
|
|
// zero or denorm -> zero
|
|
mantissa = 0;
|
|
}
|
|
else if (exponent == 255 && mantissa != 0)
|
|
{
|
|
// nan -> infinity
|
|
exponent = 31;
|
|
mantissa = 0;
|
|
}
|
|
else if (exponent >= 127 - 15 + 31)
|
|
{
|
|
// overflow or infinity -> infinity
|
|
exponent = 31;
|
|
mantissa = 0;
|
|
}
|
|
else if (exponent <= 127 - 15)
|
|
{
|
|
// underflow -> zero
|
|
exponent = 0;
|
|
mantissa = 0;
|
|
}
|
|
else
|
|
{
|
|
exponent -= 127 - 15;
|
|
mantissa >>= 13;
|
|
}
|
|
|
|
hfptr[i] = (unsigned short)((sign << 15) | (exponent << 10) | mantissa);
|
|
}
|
|
}
|
|
|
|
inline Vector4 &Vector4::operator=(const Vector4 &vec)
|
|
{
|
|
mX = vec.mX;
|
|
mY = vec.mY;
|
|
mZ = vec.mZ;
|
|
mW = vec.mW;
|
|
return *this;
|
|
}
|
|
|
|
inline Vector4 &Vector4::setXYZ(const Vector3 &vec)
|
|
{
|
|
mX = vec.getX();
|
|
mY = vec.getY();
|
|
mZ = vec.getZ();
|
|
return *this;
|
|
}
|
|
|
|
inline const Vector3 Vector4::getXYZ() const
|
|
{
|
|
return Vector3(mX, mY, mZ);
|
|
}
|
|
|
|
inline Vector4 &Vector4::setX(float _x)
|
|
{
|
|
mX = _x;
|
|
return *this;
|
|
}
|
|
|
|
inline float Vector4::getX() const
|
|
{
|
|
return mX;
|
|
}
|
|
|
|
inline Vector4 &Vector4::setY(float _y)
|
|
{
|
|
mY = _y;
|
|
return *this;
|
|
}
|
|
|
|
inline float Vector4::getY() const
|
|
{
|
|
return mY;
|
|
}
|
|
|
|
inline Vector4 &Vector4::setZ(float _z)
|
|
{
|
|
mZ = _z;
|
|
return *this;
|
|
}
|
|
|
|
inline float Vector4::getZ() const
|
|
{
|
|
return mZ;
|
|
}
|
|
|
|
inline Vector4 &Vector4::setW(float _w)
|
|
{
|
|
mW = _w;
|
|
return *this;
|
|
}
|
|
|
|
inline float Vector4::getW() const
|
|
{
|
|
return mW;
|
|
}
|
|
|
|
inline Vector4 &Vector4::setElem(int idx, float value)
|
|
{
|
|
*(&mX + idx) = value;
|
|
return *this;
|
|
}
|
|
|
|
inline float Vector4::getElem(int idx) const
|
|
{
|
|
return *(&mX + idx);
|
|
}
|
|
|
|
inline float &Vector4::operator[](int idx)
|
|
{
|
|
return *(&mX + idx);
|
|
}
|
|
|
|
inline float Vector4::operator[](int idx) const
|
|
{
|
|
return *(&mX + idx);
|
|
}
|
|
|
|
inline const Vector4 Vector4::operator+(const Vector4 &vec) const
|
|
{
|
|
return Vector4(
|
|
(mX + vec.mX),
|
|
(mY + vec.mY),
|
|
(mZ + vec.mZ),
|
|
(mW + vec.mW));
|
|
}
|
|
|
|
inline const Vector4 Vector4::operator-(const Vector4 &vec) const
|
|
{
|
|
return Vector4(
|
|
(mX - vec.mX),
|
|
(mY - vec.mY),
|
|
(mZ - vec.mZ),
|
|
(mW - vec.mW));
|
|
}
|
|
|
|
inline const Vector4 Vector4::operator*(float scalar) const
|
|
{
|
|
return Vector4(
|
|
(mX * scalar),
|
|
(mY * scalar),
|
|
(mZ * scalar),
|
|
(mW * scalar));
|
|
}
|
|
|
|
inline Vector4 &Vector4::operator+=(const Vector4 &vec)
|
|
{
|
|
*this = *this + vec;
|
|
return *this;
|
|
}
|
|
|
|
inline Vector4 &Vector4::operator-=(const Vector4 &vec)
|
|
{
|
|
*this = *this - vec;
|
|
return *this;
|
|
}
|
|
|
|
inline Vector4 &Vector4::operator*=(float scalar)
|
|
{
|
|
*this = *this * scalar;
|
|
return *this;
|
|
}
|
|
|
|
inline const Vector4 Vector4::operator/(float scalar) const
|
|
{
|
|
return Vector4(
|
|
(mX / scalar),
|
|
(mY / scalar),
|
|
(mZ / scalar),
|
|
(mW / scalar));
|
|
}
|
|
|
|
inline Vector4 &Vector4::operator/=(float scalar)
|
|
{
|
|
*this = *this / scalar;
|
|
return *this;
|
|
}
|
|
|
|
inline const Vector4 Vector4::operator-() const
|
|
{
|
|
return Vector4(
|
|
-mX,
|
|
-mY,
|
|
-mZ,
|
|
-mW);
|
|
}
|
|
|
|
inline const Vector4 operator*(float scalar, const Vector4 &vec)
|
|
{
|
|
return vec * scalar;
|
|
}
|
|
|
|
inline const Vector4 mulPerElem(const Vector4 &vec0, const Vector4 &vec1)
|
|
{
|
|
return Vector4(
|
|
(vec0.getX() * vec1.getX()),
|
|
(vec0.getY() * vec1.getY()),
|
|
(vec0.getZ() * vec1.getZ()),
|
|
(vec0.getW() * vec1.getW()));
|
|
}
|
|
|
|
inline const Vector4 divPerElem(const Vector4 &vec0, const Vector4 &vec1)
|
|
{
|
|
return Vector4(
|
|
(vec0.getX() / vec1.getX()),
|
|
(vec0.getY() / vec1.getY()),
|
|
(vec0.getZ() / vec1.getZ()),
|
|
(vec0.getW() / vec1.getW()));
|
|
}
|
|
|
|
inline const Vector4 recipPerElem(const Vector4 &vec)
|
|
{
|
|
return Vector4(
|
|
(1.0f / vec.getX()),
|
|
(1.0f / vec.getY()),
|
|
(1.0f / vec.getZ()),
|
|
(1.0f / vec.getW()));
|
|
}
|
|
|
|
inline const Vector4 sqrtPerElem(const Vector4 &vec)
|
|
{
|
|
return Vector4(
|
|
sqrtf(vec.getX()),
|
|
sqrtf(vec.getY()),
|
|
sqrtf(vec.getZ()),
|
|
sqrtf(vec.getW()));
|
|
}
|
|
|
|
inline const Vector4 rsqrtPerElem(const Vector4 &vec)
|
|
{
|
|
return Vector4(
|
|
(1.0f / sqrtf(vec.getX())),
|
|
(1.0f / sqrtf(vec.getY())),
|
|
(1.0f / sqrtf(vec.getZ())),
|
|
(1.0f / sqrtf(vec.getW())));
|
|
}
|
|
|
|
inline const Vector4 absPerElem(const Vector4 &vec)
|
|
{
|
|
return Vector4(
|
|
fabsf(vec.getX()),
|
|
fabsf(vec.getY()),
|
|
fabsf(vec.getZ()),
|
|
fabsf(vec.getW()));
|
|
}
|
|
|
|
inline const Vector4 copySignPerElem(const Vector4 &vec0, const Vector4 &vec1)
|
|
{
|
|
return Vector4(
|
|
(vec1.getX() < 0.0f) ? -fabsf(vec0.getX()) : fabsf(vec0.getX()),
|
|
(vec1.getY() < 0.0f) ? -fabsf(vec0.getY()) : fabsf(vec0.getY()),
|
|
(vec1.getZ() < 0.0f) ? -fabsf(vec0.getZ()) : fabsf(vec0.getZ()),
|
|
(vec1.getW() < 0.0f) ? -fabsf(vec0.getW()) : fabsf(vec0.getW()));
|
|
}
|
|
|
|
inline const Vector4 maxPerElem(const Vector4 &vec0, const Vector4 &vec1)
|
|
{
|
|
return Vector4(
|
|
(vec0.getX() > vec1.getX()) ? vec0.getX() : vec1.getX(),
|
|
(vec0.getY() > vec1.getY()) ? vec0.getY() : vec1.getY(),
|
|
(vec0.getZ() > vec1.getZ()) ? vec0.getZ() : vec1.getZ(),
|
|
(vec0.getW() > vec1.getW()) ? vec0.getW() : vec1.getW());
|
|
}
|
|
|
|
inline float maxElem(const Vector4 &vec)
|
|
{
|
|
float result;
|
|
result = (vec.getX() > vec.getY()) ? vec.getX() : vec.getY();
|
|
result = (vec.getZ() > result) ? vec.getZ() : result;
|
|
result = (vec.getW() > result) ? vec.getW() : result;
|
|
return result;
|
|
}
|
|
|
|
inline const Vector4 minPerElem(const Vector4 &vec0, const Vector4 &vec1)
|
|
{
|
|
return Vector4(
|
|
(vec0.getX() < vec1.getX()) ? vec0.getX() : vec1.getX(),
|
|
(vec0.getY() < vec1.getY()) ? vec0.getY() : vec1.getY(),
|
|
(vec0.getZ() < vec1.getZ()) ? vec0.getZ() : vec1.getZ(),
|
|
(vec0.getW() < vec1.getW()) ? vec0.getW() : vec1.getW());
|
|
}
|
|
|
|
inline float minElem(const Vector4 &vec)
|
|
{
|
|
float result;
|
|
result = (vec.getX() < vec.getY()) ? vec.getX() : vec.getY();
|
|
result = (vec.getZ() < result) ? vec.getZ() : result;
|
|
result = (vec.getW() < result) ? vec.getW() : result;
|
|
return result;
|
|
}
|
|
|
|
inline float sum(const Vector4 &vec)
|
|
{
|
|
float result;
|
|
result = (vec.getX() + vec.getY());
|
|
result = (result + vec.getZ());
|
|
result = (result + vec.getW());
|
|
return result;
|
|
}
|
|
|
|
inline float dot(const Vector4 &vec0, const Vector4 &vec1)
|
|
{
|
|
float result;
|
|
result = (vec0.getX() * vec1.getX());
|
|
result = (result + (vec0.getY() * vec1.getY()));
|
|
result = (result + (vec0.getZ() * vec1.getZ()));
|
|
result = (result + (vec0.getW() * vec1.getW()));
|
|
return result;
|
|
}
|
|
|
|
inline float lengthSqr(const Vector4 &vec)
|
|
{
|
|
float result;
|
|
result = (vec.getX() * vec.getX());
|
|
result = (result + (vec.getY() * vec.getY()));
|
|
result = (result + (vec.getZ() * vec.getZ()));
|
|
result = (result + (vec.getW() * vec.getW()));
|
|
return result;
|
|
}
|
|
|
|
inline float length(const Vector4 &vec)
|
|
{
|
|
return ::sqrtf(lengthSqr(vec));
|
|
}
|
|
|
|
inline const Vector4 normalize(const Vector4 &vec)
|
|
{
|
|
float lenSqr, lenInv;
|
|
lenSqr = lengthSqr(vec);
|
|
lenInv = (1.0f / sqrtf(lenSqr));
|
|
return Vector4(
|
|
(vec.getX() * lenInv),
|
|
(vec.getY() * lenInv),
|
|
(vec.getZ() * lenInv),
|
|
(vec.getW() * lenInv));
|
|
}
|
|
|
|
inline const Vector4 select(const Vector4 &vec0, const Vector4 &vec1, bool select1)
|
|
{
|
|
return Vector4(
|
|
(select1) ? vec1.getX() : vec0.getX(),
|
|
(select1) ? vec1.getY() : vec0.getY(),
|
|
(select1) ? vec1.getZ() : vec0.getZ(),
|
|
(select1) ? vec1.getW() : vec0.getW());
|
|
}
|
|
|
|
#ifdef _VECTORMATH_DEBUG
|
|
|
|
inline void print(const Vector4 &vec)
|
|
{
|
|
printf("( %f %f %f %f )\n", vec.getX(), vec.getY(), vec.getZ(), vec.getW());
|
|
}
|
|
|
|
inline void print(const Vector4 &vec, const char *name)
|
|
{
|
|
printf("%s: ( %f %f %f %f )\n", name, vec.getX(), vec.getY(), vec.getZ(), vec.getW());
|
|
}
|
|
|
|
#endif
|
|
|
|
inline Point3::Point3(const Point3 &pnt)
|
|
{
|
|
mX = pnt.mX;
|
|
mY = pnt.mY;
|
|
mZ = pnt.mZ;
|
|
}
|
|
|
|
inline Point3::Point3(float _x, float _y, float _z)
|
|
{
|
|
mX = _x;
|
|
mY = _y;
|
|
mZ = _z;
|
|
}
|
|
|
|
inline Point3::Point3(const Vector3 &vec)
|
|
{
|
|
mX = vec.getX();
|
|
mY = vec.getY();
|
|
mZ = vec.getZ();
|
|
}
|
|
|
|
inline Point3::Point3(float scalar)
|
|
{
|
|
mX = scalar;
|
|
mY = scalar;
|
|
mZ = scalar;
|
|
}
|
|
|
|
inline const Point3 lerp(float t, const Point3 &pnt0, const Point3 &pnt1)
|
|
{
|
|
return (pnt0 + ((pnt1 - pnt0) * t));
|
|
}
|
|
|
|
inline void loadXYZ(Point3 &pnt, const float *fptr)
|
|
{
|
|
pnt = Point3(fptr[0], fptr[1], fptr[2]);
|
|
}
|
|
|
|
inline void storeXYZ(const Point3 &pnt, float *fptr)
|
|
{
|
|
fptr[0] = pnt.getX();
|
|
fptr[1] = pnt.getY();
|
|
fptr[2] = pnt.getZ();
|
|
}
|
|
|
|
inline void loadHalfFloats(Point3 &vec, const unsigned short *hfptr)
|
|
{
|
|
union Data32 {
|
|
unsigned int u32;
|
|
float f32;
|
|
};
|
|
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
unsigned short fp16 = hfptr[i];
|
|
unsigned int sign = fp16 >> 15;
|
|
unsigned int exponent = (fp16 >> 10) & ((1 << 5) - 1);
|
|
unsigned int mantissa = fp16 & ((1 << 10) - 1);
|
|
|
|
if (exponent == 0)
|
|
{
|
|
// zero
|
|
mantissa = 0;
|
|
}
|
|
else if (exponent == 31)
|
|
{
|
|
// infinity or nan -> infinity
|
|
exponent = 255;
|
|
mantissa = 0;
|
|
}
|
|
else
|
|
{
|
|
exponent += 127 - 15;
|
|
mantissa <<= 13;
|
|
}
|
|
|
|
Data32 d;
|
|
d.u32 = (sign << 31) | (exponent << 23) | mantissa;
|
|
vec[i] = d.f32;
|
|
}
|
|
}
|
|
|
|
inline void storeHalfFloats(const Point3 &vec, unsigned short *hfptr)
|
|
{
|
|
union Data32 {
|
|
unsigned int u32;
|
|
float f32;
|
|
};
|
|
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
Data32 d;
|
|
d.f32 = vec[i];
|
|
|
|
unsigned int sign = d.u32 >> 31;
|
|
unsigned int exponent = (d.u32 >> 23) & ((1 << 8) - 1);
|
|
unsigned int mantissa = d.u32 & ((1 << 23) - 1);
|
|
;
|
|
|
|
if (exponent == 0)
|
|
{
|
|
// zero or denorm -> zero
|
|
mantissa = 0;
|
|
}
|
|
else if (exponent == 255 && mantissa != 0)
|
|
{
|
|
// nan -> infinity
|
|
exponent = 31;
|
|
mantissa = 0;
|
|
}
|
|
else if (exponent >= 127 - 15 + 31)
|
|
{
|
|
// overflow or infinity -> infinity
|
|
exponent = 31;
|
|
mantissa = 0;
|
|
}
|
|
else if (exponent <= 127 - 15)
|
|
{
|
|
// underflow -> zero
|
|
exponent = 0;
|
|
mantissa = 0;
|
|
}
|
|
else
|
|
{
|
|
exponent -= 127 - 15;
|
|
mantissa >>= 13;
|
|
}
|
|
|
|
hfptr[i] = (unsigned short)((sign << 15) | (exponent << 10) | mantissa);
|
|
}
|
|
}
|
|
|
|
inline Point3 &Point3::operator=(const Point3 &pnt)
|
|
{
|
|
mX = pnt.mX;
|
|
mY = pnt.mY;
|
|
mZ = pnt.mZ;
|
|
return *this;
|
|
}
|
|
|
|
inline Point3 &Point3::setX(float _x)
|
|
{
|
|
mX = _x;
|
|
return *this;
|
|
}
|
|
|
|
inline float Point3::getX() const
|
|
{
|
|
return mX;
|
|
}
|
|
|
|
inline Point3 &Point3::setY(float _y)
|
|
{
|
|
mY = _y;
|
|
return *this;
|
|
}
|
|
|
|
inline float Point3::getY() const
|
|
{
|
|
return mY;
|
|
}
|
|
|
|
inline Point3 &Point3::setZ(float _z)
|
|
{
|
|
mZ = _z;
|
|
return *this;
|
|
}
|
|
|
|
inline float Point3::getZ() const
|
|
{
|
|
return mZ;
|
|
}
|
|
|
|
inline Point3 &Point3::setElem(int idx, float value)
|
|
{
|
|
*(&mX + idx) = value;
|
|
return *this;
|
|
}
|
|
|
|
inline float Point3::getElem(int idx) const
|
|
{
|
|
return *(&mX + idx);
|
|
}
|
|
|
|
inline float &Point3::operator[](int idx)
|
|
{
|
|
return *(&mX + idx);
|
|
}
|
|
|
|
inline float Point3::operator[](int idx) const
|
|
{
|
|
return *(&mX + idx);
|
|
}
|
|
|
|
inline const Vector3 Point3::operator-(const Point3 &pnt) const
|
|
{
|
|
return Vector3(
|
|
(mX - pnt.mX),
|
|
(mY - pnt.mY),
|
|
(mZ - pnt.mZ));
|
|
}
|
|
|
|
inline const Point3 Point3::operator+(const Vector3 &vec) const
|
|
{
|
|
return Point3(
|
|
(mX + vec.getX()),
|
|
(mY + vec.getY()),
|
|
(mZ + vec.getZ()));
|
|
}
|
|
|
|
inline const Point3 Point3::operator-(const Vector3 &vec) const
|
|
{
|
|
return Point3(
|
|
(mX - vec.getX()),
|
|
(mY - vec.getY()),
|
|
(mZ - vec.getZ()));
|
|
}
|
|
|
|
inline Point3 &Point3::operator+=(const Vector3 &vec)
|
|
{
|
|
*this = *this + vec;
|
|
return *this;
|
|
}
|
|
|
|
inline Point3 &Point3::operator-=(const Vector3 &vec)
|
|
{
|
|
*this = *this - vec;
|
|
return *this;
|
|
}
|
|
|
|
inline const Point3 mulPerElem(const Point3 &pnt0, const Point3 &pnt1)
|
|
{
|
|
return Point3(
|
|
(pnt0.getX() * pnt1.getX()),
|
|
(pnt0.getY() * pnt1.getY()),
|
|
(pnt0.getZ() * pnt1.getZ()));
|
|
}
|
|
|
|
inline const Point3 divPerElem(const Point3 &pnt0, const Point3 &pnt1)
|
|
{
|
|
return Point3(
|
|
(pnt0.getX() / pnt1.getX()),
|
|
(pnt0.getY() / pnt1.getY()),
|
|
(pnt0.getZ() / pnt1.getZ()));
|
|
}
|
|
|
|
inline const Point3 recipPerElem(const Point3 &pnt)
|
|
{
|
|
return Point3(
|
|
(1.0f / pnt.getX()),
|
|
(1.0f / pnt.getY()),
|
|
(1.0f / pnt.getZ()));
|
|
}
|
|
|
|
inline const Point3 sqrtPerElem(const Point3 &pnt)
|
|
{
|
|
return Point3(
|
|
sqrtf(pnt.getX()),
|
|
sqrtf(pnt.getY()),
|
|
sqrtf(pnt.getZ()));
|
|
}
|
|
|
|
inline const Point3 rsqrtPerElem(const Point3 &pnt)
|
|
{
|
|
return Point3(
|
|
(1.0f / sqrtf(pnt.getX())),
|
|
(1.0f / sqrtf(pnt.getY())),
|
|
(1.0f / sqrtf(pnt.getZ())));
|
|
}
|
|
|
|
inline const Point3 absPerElem(const Point3 &pnt)
|
|
{
|
|
return Point3(
|
|
fabsf(pnt.getX()),
|
|
fabsf(pnt.getY()),
|
|
fabsf(pnt.getZ()));
|
|
}
|
|
|
|
inline const Point3 copySignPerElem(const Point3 &pnt0, const Point3 &pnt1)
|
|
{
|
|
return Point3(
|
|
(pnt1.getX() < 0.0f) ? -fabsf(pnt0.getX()) : fabsf(pnt0.getX()),
|
|
(pnt1.getY() < 0.0f) ? -fabsf(pnt0.getY()) : fabsf(pnt0.getY()),
|
|
(pnt1.getZ() < 0.0f) ? -fabsf(pnt0.getZ()) : fabsf(pnt0.getZ()));
|
|
}
|
|
|
|
inline const Point3 maxPerElem(const Point3 &pnt0, const Point3 &pnt1)
|
|
{
|
|
return Point3(
|
|
(pnt0.getX() > pnt1.getX()) ? pnt0.getX() : pnt1.getX(),
|
|
(pnt0.getY() > pnt1.getY()) ? pnt0.getY() : pnt1.getY(),
|
|
(pnt0.getZ() > pnt1.getZ()) ? pnt0.getZ() : pnt1.getZ());
|
|
}
|
|
|
|
inline float maxElem(const Point3 &pnt)
|
|
{
|
|
float result;
|
|
result = (pnt.getX() > pnt.getY()) ? pnt.getX() : pnt.getY();
|
|
result = (pnt.getZ() > result) ? pnt.getZ() : result;
|
|
return result;
|
|
}
|
|
|
|
inline const Point3 minPerElem(const Point3 &pnt0, const Point3 &pnt1)
|
|
{
|
|
return Point3(
|
|
(pnt0.getX() < pnt1.getX()) ? pnt0.getX() : pnt1.getX(),
|
|
(pnt0.getY() < pnt1.getY()) ? pnt0.getY() : pnt1.getY(),
|
|
(pnt0.getZ() < pnt1.getZ()) ? pnt0.getZ() : pnt1.getZ());
|
|
}
|
|
|
|
inline float minElem(const Point3 &pnt)
|
|
{
|
|
float result;
|
|
result = (pnt.getX() < pnt.getY()) ? pnt.getX() : pnt.getY();
|
|
result = (pnt.getZ() < result) ? pnt.getZ() : result;
|
|
return result;
|
|
}
|
|
|
|
inline float sum(const Point3 &pnt)
|
|
{
|
|
float result;
|
|
result = (pnt.getX() + pnt.getY());
|
|
result = (result + pnt.getZ());
|
|
return result;
|
|
}
|
|
|
|
inline const Point3 scale(const Point3 &pnt, float scaleVal)
|
|
{
|
|
return mulPerElem(pnt, Point3(scaleVal));
|
|
}
|
|
|
|
inline const Point3 scale(const Point3 &pnt, const Vector3 &scaleVec)
|
|
{
|
|
return mulPerElem(pnt, Point3(scaleVec));
|
|
}
|
|
|
|
inline float projection(const Point3 &pnt, const Vector3 &unitVec)
|
|
{
|
|
float result;
|
|
result = (pnt.getX() * unitVec.getX());
|
|
result = (result + (pnt.getY() * unitVec.getY()));
|
|
result = (result + (pnt.getZ() * unitVec.getZ()));
|
|
return result;
|
|
}
|
|
|
|
inline float distSqrFromOrigin(const Point3 &pnt)
|
|
{
|
|
return lengthSqr(Vector3(pnt));
|
|
}
|
|
|
|
inline float distFromOrigin(const Point3 &pnt)
|
|
{
|
|
return length(Vector3(pnt));
|
|
}
|
|
|
|
inline float distSqr(const Point3 &pnt0, const Point3 &pnt1)
|
|
{
|
|
return lengthSqr((pnt1 - pnt0));
|
|
}
|
|
|
|
inline float dist(const Point3 &pnt0, const Point3 &pnt1)
|
|
{
|
|
return length((pnt1 - pnt0));
|
|
}
|
|
|
|
inline const Point3 select(const Point3 &pnt0, const Point3 &pnt1, bool select1)
|
|
{
|
|
return Point3(
|
|
(select1) ? pnt1.getX() : pnt0.getX(),
|
|
(select1) ? pnt1.getY() : pnt0.getY(),
|
|
(select1) ? pnt1.getZ() : pnt0.getZ());
|
|
}
|
|
|
|
#ifdef _VECTORMATH_DEBUG
|
|
|
|
inline void print(const Point3 &pnt)
|
|
{
|
|
printf("( %f %f %f )\n", pnt.getX(), pnt.getY(), pnt.getZ());
|
|
}
|
|
|
|
inline void print(const Point3 &pnt, const char *name)
|
|
{
|
|
printf("%s: ( %f %f %f )\n", name, pnt.getX(), pnt.getY(), pnt.getZ());
|
|
}
|
|
|
|
#endif
|
|
|
|
} // namespace Aos
|
|
} // namespace Vectormath
|
|
|
|
#endif
|