1428 lines
38 KiB
C++
1428 lines
38 KiB
C++
#ifndef LifeMath_h__
|
|
#define LifeMath_h__
|
|
|
|
//#include "LifeDebug.h"
|
|
|
|
#include "Math.h"
|
|
#include <cmath>
|
|
#include <algorithm>
|
|
#include "Vector2D.h"
|
|
#include "Vector3D.h"
|
|
#include "Vector4D.h"
|
|
#include "Matrix4x4.h"
|
|
#include "Quaternion.h"
|
|
#include "Plane.h"
|
|
#include <intrin.h>
|
|
|
|
inline int GetFlag(int val, int flag)
|
|
{
|
|
return val & flag;
|
|
}
|
|
inline void RemoveFlag(int& val, int flag)
|
|
{
|
|
val = val & (~flag);
|
|
}
|
|
inline void SetFlag(int& val, int flag)
|
|
{
|
|
val |= flag;
|
|
}
|
|
namespace LifeMath
|
|
{
|
|
#undef INLINE
|
|
#ifdef _WIN32
|
|
#ifdef _MSC_VER
|
|
#define INLINE __forceinline
|
|
#endif
|
|
#else
|
|
#ifdef __GNUC__
|
|
#define INLINE __attribute__((always_inline))
|
|
#endif
|
|
#endif
|
|
|
|
#ifndef INLINE
|
|
#define INLINE inline
|
|
#endif
|
|
|
|
/*#define min(x, y) \
|
|
(x < y ? x : y)
|
|
|
|
#define max(x, y) \
|
|
(x > y ? x : y)*/
|
|
|
|
/*template<typename T>
|
|
INLINE const T& (min)(const T& left, const T& right)
|
|
{
|
|
return left < right ? left : right;
|
|
}
|
|
|
|
template<typename T>
|
|
INLINE T max(T left, T right)
|
|
{
|
|
return left > right ? left : right;
|
|
}*/
|
|
|
|
static const float PI = 3.14159265358979f;
|
|
enum Axis
|
|
{
|
|
X, Y, Z, W
|
|
};
|
|
|
|
/*template<typename T>
|
|
T SetFlag(T val, T flag)
|
|
{
|
|
return val | flag;
|
|
}
|
|
|
|
template<typename T>
|
|
T GetFlag(T val, T flag)
|
|
{
|
|
return val & flag;
|
|
}*/
|
|
|
|
/*template<typename T>
|
|
T RemoveFlag(T val, T flag)
|
|
{
|
|
return val & (~flag);
|
|
}*/
|
|
|
|
extern void Mat4x4Invert(const Matrix4x4* matrix, Matrix4x4* result);
|
|
extern void QuatRotationYawPitchRoll(float yaw, float pitch, float roll, Quaternion& result);
|
|
extern void QuatRotationMatrix(const Matrix4x4& matrix, Quaternion& dst);
|
|
|
|
//----- Vector3D-----
|
|
INLINE float Vec3Length(Vec3f* src)
|
|
{
|
|
return sqrtf(src->X * src->X + src->Y * src->Y + src->Z * src->Z);
|
|
}
|
|
|
|
INLINE float Vec3Length(const Vec3f& src)
|
|
{
|
|
return sqrtf(src.X * src.X + src.Y * src.Y + src.Z * src.Z);
|
|
}
|
|
|
|
INLINE void Vec3Normalize(Vec3f* src)
|
|
{
|
|
float num2 = Vec3Length(src);
|
|
if (num2 != 0.0f)
|
|
{
|
|
float num = 1.0f / num2;
|
|
src->X *= num;
|
|
src->Y *= num;
|
|
src->Z *= num;
|
|
}
|
|
}
|
|
|
|
INLINE void Vec3Normalize(Vec3f* vector, Vec3f* result)
|
|
{
|
|
float num2 = Vec3Length(vector);
|
|
if (num2 != 0.0f)
|
|
{
|
|
float num = 1.0f / num2;
|
|
result->X = vector->X * num;
|
|
result->Y = vector->Y * num;
|
|
result->Z = vector->Z * num;
|
|
}
|
|
else
|
|
{
|
|
result->X = result->Y = result->Z = 0.0f;
|
|
}
|
|
}
|
|
|
|
INLINE void Vec3Add(Vec3f& src, Vec3f& val)
|
|
{
|
|
src.X += val.X;
|
|
src.Y += val.Y;
|
|
src.Z += val.Z;
|
|
}
|
|
|
|
INLINE void Vec3Reflect(Vec3f& vector, Vec3f& normal, Vec3f& result)
|
|
{
|
|
float num2 = ((vector.Y * normal.Y) + (vector.X * normal.X)) + (vector.Z * normal.Z);
|
|
float num = num2 * 2.0f;
|
|
result.X = vector.X - (normal.X * num);
|
|
result.Y = vector.Y - (normal.Y * num);
|
|
result.Z = vector.Z - (normal.Z * num);
|
|
}
|
|
|
|
INLINE void Vec3Reflect(Vec3f& vector, Vec3f& normal)
|
|
{
|
|
float num2 = ((vector.Y * normal.Y) + (vector.X * normal.X)) + (vector.Z * normal.Z);
|
|
float num = num2 * 2.0f;
|
|
vector.X = vector.X - (normal.X * num);
|
|
vector.Y = vector.Y - (normal.Y * num);
|
|
vector.Z = vector.Z - (normal.Z * num);
|
|
}
|
|
|
|
INLINE void Vec3Transform(Vec3f* vector, Matrix4x4* transformation, Vec4f* result)
|
|
{
|
|
result->X = (((vector->Y * transformation->M21) + (vector->X * transformation->M11)) + (vector->Z * transformation->M31)) + transformation->M41;
|
|
result->Y = (((vector->Y * transformation->M22) + (vector->X * transformation->M12)) + (vector->Z * transformation->M32)) + transformation->M42;
|
|
result->Z = (((vector->Y * transformation->M23) + (vector->X * transformation->M13)) + (vector->Z * transformation->M33)) + transformation->M43;
|
|
result->W = (((transformation->M24 * vector->Y) + (transformation->M14 * vector->X)) + (vector->Z * transformation->M34)) + transformation->M44;
|
|
}
|
|
|
|
INLINE float Vec3Dot(const Vec3f* left, const Vec3f* right)
|
|
{
|
|
return (((left->Y * right->Y) + (left->X * right->X)) + (left->Z * right->Z));
|
|
}
|
|
|
|
INLINE Vec3f Vec3TransformCoordinate(const Matrix4x4& transformation, const Vec3f& coordinate)
|
|
{
|
|
Vec3f vector;
|
|
Vec4f vector2;
|
|
vector2.X = (((transformation.M21 * coordinate.Y) + (transformation.M11 * coordinate.X)) + (transformation.M31 * coordinate.Z)) + transformation.M41;
|
|
vector2.Y = (((transformation.M22 * coordinate.Y) + (transformation.M12 * coordinate.X)) + (transformation.M32 * coordinate.Z)) + transformation.M42;
|
|
vector2.Z = (((transformation.M23 * coordinate.Y) + (transformation.M13 * coordinate.X)) + (transformation.M33 * coordinate.Z)) + transformation.M43;
|
|
float num = (1.0f / ((((transformation.M24 * coordinate.Y) + (transformation.M14 * coordinate.X)) + (transformation.M34 * coordinate.Z)) + transformation.M44));
|
|
vector2.W = num;
|
|
vector.X = vector2.X * num;
|
|
vector.Y = vector2.Y * num;
|
|
vector.Z = vector2.Z * num;
|
|
return vector;
|
|
}
|
|
|
|
INLINE void Vec3TransformCoordinate(Vec3f& coordinate, const Matrix4x4& transformation)
|
|
{
|
|
Vec4f vector2;
|
|
vector2.X = (((transformation.M21 * coordinate.Y) + (transformation.M11 * coordinate.X)) + (transformation.M31 * coordinate.Z)) + transformation.M41;
|
|
vector2.Y = (((transformation.M22 * coordinate.Y) + (transformation.M12 * coordinate.X)) + (transformation.M32 * coordinate.Z)) + transformation.M42;
|
|
vector2.Z = (((transformation.M23 * coordinate.Y) + (transformation.M13 * coordinate.X)) + (transformation.M33 * coordinate.Z)) + transformation.M43;
|
|
float num = (float)(1.0 / ((((transformation.M24 * coordinate.Y) + (transformation.M14 * coordinate.X)) + (transformation.M34 * coordinate.Z)) + transformation.M44));
|
|
vector2.W = num;
|
|
coordinate.X = vector2.X * num;
|
|
coordinate.Y = vector2.Y * num;
|
|
coordinate.Z = vector2.Z * num;
|
|
}
|
|
|
|
INLINE Vec3f Vec3TransformNoW(const Vec3f& coordinate, const Matrix4x4& transformation)
|
|
{
|
|
Vec3f vector;
|
|
vector.X = (((transformation.M21 * coordinate.Y) + (transformation.M11 * coordinate.X)) + (transformation.M31 * coordinate.Z)) + transformation.M41;
|
|
vector.Y = (((transformation.M22 * coordinate.Y) + (transformation.M12 * coordinate.X)) + (transformation.M32 * coordinate.Z)) + transformation.M42;
|
|
vector.Z = (((transformation.M23 * coordinate.Y) + (transformation.M13 * coordinate.X)) + (transformation.M33 * coordinate.Z)) + transformation.M43;
|
|
return vector;
|
|
}
|
|
|
|
INLINE void Vec3Minimize(const Vec3f& value1, const Vec3f& value2, Vec3f& result)
|
|
{
|
|
float z;
|
|
float y;
|
|
float x;
|
|
if (value1.X < value2.X)
|
|
{
|
|
x = value1.X;
|
|
}
|
|
else
|
|
{
|
|
x = value2.X;
|
|
}
|
|
result.X = x;
|
|
if (value1.Y < value2.Y)
|
|
{
|
|
y = value1.Y;
|
|
}
|
|
else
|
|
{
|
|
y = value2.Y;
|
|
}
|
|
result.Y = y;
|
|
if (value1.Z < value2.Z)
|
|
{
|
|
z = value1.Z;
|
|
}
|
|
else
|
|
{
|
|
z = value2.Z;
|
|
}
|
|
result.Z = z;
|
|
}
|
|
|
|
INLINE void Vec3Minimize(Vec4f& value1, Vec3f& value2, Vec3f& result)
|
|
{
|
|
float z;
|
|
float y;
|
|
float x;
|
|
if (value1.X < value2.X)
|
|
{
|
|
x = value1.X;
|
|
}
|
|
else
|
|
{
|
|
x = value2.X;
|
|
}
|
|
result.X = x;
|
|
if (value1.Y < value2.Y)
|
|
{
|
|
y = value1.Y;
|
|
}
|
|
else
|
|
{
|
|
y = value2.Y;
|
|
}
|
|
result.Y = y;
|
|
if (value1.Z < value2.Z)
|
|
{
|
|
z = value1.Z;
|
|
}
|
|
else
|
|
{
|
|
z = value2.Z;
|
|
}
|
|
result.Z = z;
|
|
}
|
|
|
|
INLINE void Vec3Maximize(const Vec3f& value1, const Vec3f& value2, Vec3f& result)
|
|
{
|
|
float z;
|
|
float y;
|
|
float x;
|
|
if (value1.X > value2.X)
|
|
{
|
|
x = value1.X;
|
|
}
|
|
else
|
|
{
|
|
x = value2.X;
|
|
}
|
|
result.X = x;
|
|
if (value1.Y > value2.Y)
|
|
{
|
|
y = value1.Y;
|
|
}
|
|
else
|
|
{
|
|
y = value2.Y;
|
|
}
|
|
result.Y = y;
|
|
if (value1.Z > value2.Z)
|
|
{
|
|
z = value1.Z;
|
|
}
|
|
else
|
|
{
|
|
z = value2.Z;
|
|
}
|
|
result.Z = z;
|
|
}
|
|
|
|
INLINE void Vec3Maximize(const Vec4f& value1, const Vec3f& value2, Vec3f& result)
|
|
{
|
|
float z;
|
|
float y;
|
|
float x;
|
|
if (value1.X > value2.X)
|
|
{
|
|
x = value1.X;
|
|
}
|
|
else
|
|
{
|
|
x = value2.X;
|
|
}
|
|
result.X = x;
|
|
if (value1.Y > value2.Y)
|
|
{
|
|
y = value1.Y;
|
|
}
|
|
else
|
|
{
|
|
y = value2.Y;
|
|
}
|
|
result.Y = y;
|
|
if (value1.Z > value2.Z)
|
|
{
|
|
z = value1.Z;
|
|
}
|
|
else
|
|
{
|
|
z = value2.Z;
|
|
}
|
|
result.Z = z;
|
|
}
|
|
|
|
INLINE void Vec3TransformCoordinate(const Vec3f& coordinate, const Matrix4x4& transformation, Vec3f& result)
|
|
{
|
|
Vec4f vector2;
|
|
vector2.X = (((coordinate.Y * transformation.M21) + (coordinate.X * transformation.M11)) + (coordinate.Z * transformation.M31)) + transformation.M41;
|
|
vector2.Y = (((coordinate.Y * transformation.M22) + (coordinate.X * transformation.M12)) + (coordinate.Z * transformation.M32)) + transformation.M42;
|
|
vector2.Z = (((coordinate.Y * transformation.M23) + (coordinate.X * transformation.M13)) + (coordinate.Z * transformation.M33)) + transformation.M43;
|
|
float num = (float)(1.0 / ((((transformation.M24 * coordinate.Y) + (transformation.M14 * coordinate.X)) + (coordinate.Z * transformation.M34)) + transformation.M44));
|
|
vector2.W = num;
|
|
result.X = vector2.X * num;
|
|
result.Y = vector2.Y * num;
|
|
result.Z = vector2.Z * num;
|
|
}
|
|
|
|
INLINE void Vec3Cross(const Vec3f& left, const Vec3f& right, Vec3f& result)
|
|
{
|
|
result.X = (left.Y * right.Z) - (left.Z * right.Y);
|
|
result.Y = (left.Z * right.X) - (left.X * right.Z);
|
|
result.Z = (left.X * right.Y) - (left.Y * right.X);
|
|
}
|
|
|
|
INLINE Vec3f Vec3Cross(const Vec3f& vector1, const Vec3f& vector2)
|
|
{
|
|
Vec3f vector;
|
|
vector.X = (vector1.Y * vector2.Z) - (vector1.Z * vector2.Y);
|
|
vector.Y = (vector1.Z * vector2.X) - (vector1.X * vector2.Z);
|
|
vector.Z = (vector1.X * vector2.Y) - (vector1.Y * vector2.X);
|
|
return vector;
|
|
}
|
|
|
|
INLINE Vec3f Vec3Lerp(const Vec3f& start, const Vec3f& end, float amount)
|
|
{
|
|
Vec3f vector;
|
|
vector.X = ((end.X - start.X) * amount) + start.X;
|
|
vector.Y = ((end.Y - start.Y) * amount) + start.Y;
|
|
vector.Z = ((end.Z - start.Z) * amount) + start.Z;
|
|
return vector;
|
|
}
|
|
|
|
INLINE void Vec3Lerp(const Vec3f& start, const Vec3f& end, float amount, Vec3f& result)
|
|
{
|
|
result.X = ((end.X - start.X) * amount) + start.X;
|
|
result.Y = ((end.Y - start.Y) * amount) + start.Y;
|
|
result.Z = ((end.Z - start.Z) * amount) + start.Z;
|
|
}
|
|
|
|
INLINE void Vec3Move(Vec3f& src, const Vec3f& direction, float amount)
|
|
{
|
|
src.X += direction.X * amount;
|
|
src.Y += direction.Y * amount;
|
|
src.Z += direction.Z * amount;
|
|
}
|
|
|
|
INLINE void Vec3Mul(Vec3f& vector, float val)
|
|
{
|
|
vector.X *= val;
|
|
vector.Y *= val;
|
|
vector.Z *= val;
|
|
}
|
|
|
|
INLINE void Vec3TransformCoordinate(Vec3f& coord, Matrix4x4& transform)
|
|
{
|
|
Vec4f vector;
|
|
vector.X = (((coord.X * transform.M11) + (coord.Y * transform.M21)) + (coord.Z * transform.M31)) + transform.M41;
|
|
vector.Y = (((coord.X * transform.M12) + (coord.Y * transform.M22)) + (coord.Z * transform.M32)) + transform.M42;
|
|
vector.Z = (((coord.X * transform.M13) + (coord.Y * transform.M23)) + (coord.Z * transform.M33)) + transform.M43;
|
|
vector.W = 1.0f / ((((coord.X * transform.M14) + (coord.Y * transform.M24)) + (coord.Z * transform.M34)) + transform.M44);
|
|
//vector.W = 1.0f;
|
|
coord.X = vector.X * vector.W;
|
|
coord.Y = vector.Y * vector.W;
|
|
coord.Z = vector.Z * vector.W;
|
|
}
|
|
|
|
INLINE float Vec3Distance(const Vec3f& left, const Vec3f& right)
|
|
{
|
|
float x = right.X - left.X;
|
|
float y = right.Y - left.Y;
|
|
float z = right.Z - left.Z;
|
|
|
|
return (float)sqrtf(x * x + y * y + z * z);
|
|
}
|
|
|
|
INLINE float Vec3DistanceSquared(const Vec3f& left, const Vec3f& right)
|
|
{
|
|
float x = right.X - left.X;
|
|
float y = right.Y - left.Y;
|
|
float z = right.Z - left.Z;
|
|
|
|
return x * x + y * y + z * z;
|
|
}
|
|
|
|
INLINE float Vec3DistanceSquared(const Vec3f& left, const Vec4f& right)
|
|
{
|
|
float x = right.X - left.X;
|
|
float y = right.Y - left.Y;
|
|
float z = right.Z - left.Z;
|
|
|
|
return x * x + y * y + z * z;
|
|
}
|
|
|
|
INLINE Vec3f Vec3Unproject(const Vec3f& vector, float x, float y, float width, float height, float minZ, float maxZ, const Matrix4x4& worldViewProjection)
|
|
{
|
|
Vec3f coordinate;
|
|
Matrix4x4 transformation;
|
|
Mat4x4Invert(&worldViewProjection, &transformation);
|
|
|
|
coordinate.X = ((((vector.X - x) / (width)) * 2.0f) - 1.0f);
|
|
coordinate.Y = -((((vector.Y - y) / (height)) * 2.0f) - 1.0f);
|
|
coordinate.Z = (vector.Z - minZ) / (maxZ - minZ);
|
|
Vec3TransformCoordinate(coordinate, transformation);
|
|
|
|
return coordinate;
|
|
}
|
|
//-----Vector4D-----
|
|
INLINE void Vec4Move( Vec3f& source, Vec3f& direction, float amount)
|
|
{
|
|
source.X += direction.X * amount;
|
|
source.Y += direction.Y * amount;
|
|
source.Z += direction.Z * amount;
|
|
}
|
|
INLINE void Vec4Minimize(Vec4f& value1, Vec4f& value2, Vec4f& result)
|
|
{
|
|
float z;
|
|
float y;
|
|
float x;
|
|
float w;
|
|
if (value1.X < value2.X)
|
|
{
|
|
x = value1.X;
|
|
}
|
|
else
|
|
{
|
|
x = value2.X;
|
|
}
|
|
result.X = x;
|
|
if (value1.Y < value2.Y)
|
|
{
|
|
y = value1.Y;
|
|
}
|
|
else
|
|
{
|
|
y = value2.Y;
|
|
}
|
|
result.Y = y;
|
|
if (value1.Z < value2.Z)
|
|
{
|
|
z = value1.Z;
|
|
}
|
|
else
|
|
{
|
|
z = value2.Z;
|
|
}
|
|
result.Z = z;
|
|
if (value1.W < value2.W)
|
|
{
|
|
w = value1.W;
|
|
}
|
|
else
|
|
{
|
|
w = value2.W;
|
|
}
|
|
result.W = w;
|
|
}
|
|
INLINE void Vec4Maximize(Vec4f& value1, Vec4f& value2, Vec4f& result)
|
|
{
|
|
float z;
|
|
float y;
|
|
float x;
|
|
float w;
|
|
if (value1.X > value2.X)
|
|
{
|
|
x = value1.X;
|
|
}
|
|
else
|
|
{
|
|
x = value2.X;
|
|
}
|
|
result.X = x;
|
|
if (value1.Y > value2.Y)
|
|
{
|
|
y = value1.Y;
|
|
}
|
|
else
|
|
{
|
|
y = value2.Y;
|
|
}
|
|
result.Y = y;
|
|
if (value1.Z > value2.Z)
|
|
{
|
|
z = value1.Z;
|
|
}
|
|
else
|
|
{
|
|
z = value2.Z;
|
|
}
|
|
result.Z = z;
|
|
|
|
if (value1.W > value2.W)
|
|
{
|
|
w = value1.W;
|
|
}
|
|
else
|
|
{
|
|
w = value2.W;
|
|
}
|
|
result.W = w;
|
|
}
|
|
//-----Matrix4x4-----
|
|
INLINE Matrix4x4* Mat4x4Identity(Matrix4x4* dst)
|
|
{
|
|
memset(&dst->M12, 0, sizeof(Matrix4x4) - 2 * sizeof(float));
|
|
dst->M11 = dst->M22 = dst->M33 = dst->M44 = 1.0f;
|
|
return dst;
|
|
}
|
|
|
|
INLINE void Mat4x4Ortho(float width, float height, float zNear, float zFar, Matrix4x4& result)
|
|
{
|
|
result.M12 = result.M13 = result.M14 =
|
|
result.M21 = result.M23 = result.M24 =
|
|
result.M31 = result.M32 = result.M34 =
|
|
result.M41 = result.M42 = 0.0f;
|
|
result.M44 = 1.0f;
|
|
|
|
result.M11 = 2.0f / width;
|
|
result.M22 = 2.0f / height;
|
|
result.M33 = 1.0f / (zFar - zNear);
|
|
result.M43 = zNear / (zNear - zFar);
|
|
}
|
|
|
|
INLINE Matrix4x4 Mat4x4Invert(const Matrix4x4* matrix)
|
|
{
|
|
Matrix4x4 matrix2;
|
|
float num5 = matrix->M11;
|
|
float num4 = matrix->M12;
|
|
float num3 = matrix->M13;
|
|
float num2 = matrix->M14;
|
|
float num9 = matrix->M21;
|
|
float num8 = matrix->M22;
|
|
float num7 = matrix->M23;
|
|
float num6 = matrix->M24;
|
|
float num17 = matrix->M31;
|
|
float num16 = matrix->M32;
|
|
float num15 = matrix->M33;
|
|
float num14 = matrix->M34;
|
|
float num13 = matrix->M41;
|
|
float num12 = matrix->M42;
|
|
float num11 = matrix->M43;
|
|
float num10 = matrix->M44;
|
|
float num23 = (num15 * num10) - (num14 * num11);
|
|
float num22 = (num16 * num10) - (num14 * num12);
|
|
float num21 = (num16 * num11) - (num15 * num12);
|
|
float num20 = (num17 * num10) - (num14 * num13);
|
|
float num19 = (num17 * num11) - (num15 * num13);
|
|
float num18 = (num17 * num12) - (num16 * num13);
|
|
float num39 = ((num8 * num23) - (num7 * num22)) + (num6 * num21);
|
|
float num38 = -(((num9 * num23) - (num7 * num20)) + (num6 * num19));
|
|
float num37 = ((num9 * num22) - (num8 * num20)) + (num6 * num18);
|
|
float num36 = -(((num9 * num21) - (num8 * num19)) + (num7 * num18));
|
|
float num = 1.0f / ((((num5 * num39) + (num4 * num38)) + (num3 * num37)) + (num2 * num36));
|
|
matrix2.M11 = num39 * num;
|
|
matrix2.M21 = num38 * num;
|
|
matrix2.M31 = num37 * num;
|
|
matrix2.M41 = num36 * num;
|
|
matrix2.M12 = -(((num4 * num23) - (num3 * num22)) + (num2 * num21)) * num;
|
|
matrix2.M22 = (((num5 * num23) - (num3 * num20)) + (num2 * num19)) * num;
|
|
matrix2.M32 = -(((num5 * num22) - (num4 * num20)) + (num2 * num18)) * num;
|
|
matrix2.M42 = (((num5 * num21) - (num4 * num19)) + (num3 * num18)) * num;
|
|
float num35 = (num7 * num10) - (num6 * num11);
|
|
float num34 = (num8 * num10) - (num6 * num12);
|
|
float num33 = (num8 * num11) - (num7 * num12);
|
|
float num32 = (num9 * num10) - (num6 * num13);
|
|
float num31 = (num9 * num11) - (num7 * num13);
|
|
float num30 = (num9 * num12) - (num8 * num13);
|
|
matrix2.M13 = (((num4 * num35) - (num3 * num34)) + (num2 * num33)) * num;
|
|
matrix2.M23 = -(((num5 * num35) - (num3 * num32)) + (num2 * num31)) * num;
|
|
matrix2.M33 = (((num5 * num34) - (num4 * num32)) + (num2 * num30)) * num;
|
|
matrix2.M43 = -(((num5 * num33) - (num4 * num31)) + (num3 * num30)) * num;
|
|
float num29 = (num7 * num14) - (num6 * num15);
|
|
float num28 = (num8 * num14) - (num6 * num16);
|
|
float num27 = (num8 * num15) - (num7 * num16);
|
|
float num26 = (num9 * num14) - (num6 * num17);
|
|
float num25 = (num9 * num15) - (num7 * num17);
|
|
float num24 = (num9 * num16) - (num8 * num17);
|
|
matrix2.M14 = -(((num4 * num29) - (num3 * num28)) + (num2 * num27)) * num;
|
|
matrix2.M24 = (((num5 * num29) - (num3 * num26)) + (num2 * num25)) * num;
|
|
matrix2.M34 = -(((num5 * num28) - (num4 * num26)) + (num2 * num24)) * num;
|
|
matrix2.M44 = (((num5 * num27) - (num4 * num25)) + (num3 * num24)) * num;
|
|
return matrix2;
|
|
}
|
|
|
|
INLINE void Mat4x4MulSSE(Matrix4x4* m1, Matrix4x4* m2, Matrix4x4* m3)
|
|
{
|
|
__m128 rowW = *((__m128*)(&m1->M11));
|
|
__m128 rowX = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(0,0,0,0)));
|
|
__m128 rowY = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(1,1,1,1)));
|
|
__m128 rowZ = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(2,2,2,2)));
|
|
rowW = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(3,3,3,3)));
|
|
|
|
rowX = _mm_mul_ps(rowX,*((__m128*)(&m2->M11)));
|
|
rowY = _mm_mul_ps(rowY,*((__m128*)(&m2->M21)));
|
|
rowZ = _mm_mul_ps(rowZ,*((__m128*)(&m2->M31)));
|
|
rowW = _mm_mul_ps(rowW,*((__m128*)(&m2->M41)));
|
|
rowX = _mm_add_ps(rowX,rowZ);
|
|
rowY = _mm_add_ps(rowY,rowW);
|
|
rowX = _mm_add_ps(rowX,rowY);
|
|
*((__m128*)(&m3->M11)) = rowX;
|
|
rowW = *((__m128*)(&m1->M21));
|
|
|
|
rowX = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(0,0,0,0)));
|
|
rowY = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(1,1,1,1)));
|
|
rowZ = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(2,2,2,2)));
|
|
rowW = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(3,3,3,3)));
|
|
|
|
rowX = _mm_mul_ps(rowX,*((__m128*)(&m2->M11)));
|
|
rowY = _mm_mul_ps(rowY,*((__m128*)(&m2->M21)));
|
|
rowZ = _mm_mul_ps(rowZ,*((__m128*)(&m2->M31)));
|
|
rowW = _mm_mul_ps(rowW,*((__m128*)(&m2->M41)));
|
|
rowX = _mm_add_ps(rowX,rowZ);
|
|
rowY = _mm_add_ps(rowY,rowW);
|
|
rowX = _mm_add_ps(rowX,rowY);
|
|
*((__m128*)(&m3->M21)) = rowX;
|
|
rowW = *((__m128*)(&m1->M31));
|
|
|
|
rowX = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(0,0,0,0)));
|
|
rowY = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(1,1,1,1)));
|
|
rowZ = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(2,2,2,2)));
|
|
rowW = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(3,3,3,3)));
|
|
|
|
rowX = _mm_mul_ps(rowX,*((__m128*)(&m2->M11)));
|
|
rowY = _mm_mul_ps(rowY,*((__m128*)(&m2->M21)));
|
|
rowZ = _mm_mul_ps(rowZ,*((__m128*)(&m2->M31)));
|
|
rowW = _mm_mul_ps(rowW,*((__m128*)(&m2->M41)));
|
|
rowX = _mm_add_ps(rowX,rowZ);
|
|
rowY = _mm_add_ps(rowY,rowW);
|
|
rowX = _mm_add_ps(rowX,rowY);
|
|
*((__m128*)(&m3->M31)) = rowX;
|
|
rowW = *((__m128*)(&m1->M41));
|
|
|
|
rowX = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(0,0,0,0)));
|
|
rowY = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(1,1,1,1)));
|
|
rowZ = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(2,2,2,2)));
|
|
rowW = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(rowW),_MM_SHUFFLE(3,3,3,3)));
|
|
|
|
rowX = _mm_mul_ps(rowX,*((__m128*)(&m2->M11)));
|
|
rowY = _mm_mul_ps(rowY,*((__m128*)(&m2->M21)));
|
|
rowZ = _mm_mul_ps(rowZ,*((__m128*)(&m2->M31)));
|
|
rowW = _mm_mul_ps(rowW,*((__m128*)(&m2->M41)));
|
|
rowX = _mm_add_ps(rowX,rowZ);
|
|
rowY = _mm_add_ps(rowY,rowW);
|
|
rowX = _mm_add_ps(rowX,rowY);
|
|
*((__m128*)(&m3->M41)) = rowX;
|
|
}
|
|
|
|
INLINE Matrix4x4* Mat4x4RotationAxis(Vec3f* axis, float angle, Matrix4x4* pResult)
|
|
{
|
|
float y = axis->Y;
|
|
float x = axis->X;
|
|
float z = axis->Z;
|
|
if (((float)(((x * x) + (y * y)) + (z * z))) != 1.0f)
|
|
{
|
|
Vec3Normalize(axis);
|
|
// axis.Normalize();
|
|
}
|
|
float num4 = axis->X;
|
|
float num3 = axis->Y;
|
|
float num2 = axis->Z;
|
|
float num = cosf(angle);
|
|
float num5 = sinf(angle);
|
|
float num23 = num4;
|
|
float num17 = (num23 * num23);
|
|
float num22 = num3;
|
|
float num16 = (num22 * num22);
|
|
float num21 = num2;
|
|
float num15 = (num21 * num21);
|
|
float num20 = num3 * num4;
|
|
float num19 = num2 * num4;
|
|
float num18 = num2 * num3;
|
|
pResult->M11 = (((1.0f - num17) * num)) + num17;
|
|
float num14 = num20;
|
|
float num13 = num14 - (num * num14);
|
|
float num12 = num5 * num2;
|
|
pResult->M12 = (num12 + num13);
|
|
float num11 = num19;
|
|
float num10 = num11 - (num * num11);
|
|
float num9 = num5 * num3;
|
|
pResult->M13 = (num10 - num9);
|
|
pResult->M14 = 0.0f;
|
|
pResult->M21 = (num13 - num12);
|
|
pResult->M22 = (((1.0f - num16) * num)) + num16;
|
|
float num8 = num18;
|
|
float num7 = num8 - (num * num8);
|
|
float num6 = num5 * num4;
|
|
pResult->M23 = (float)(num6 + num7);
|
|
pResult->M24 = 0.0f;
|
|
pResult->M31 = (float)(num9 + num10);
|
|
pResult->M32 = (float)(num7 - num6);
|
|
pResult->M33 = ((float)((1.0f - num15) * num)) + num15;
|
|
pResult->M34 = 0.0f;
|
|
pResult->M41 = 0.0f;
|
|
pResult->M42 = 0.0f;
|
|
pResult->M43 = 0.0f;
|
|
pResult->M44 = 1.0f;
|
|
|
|
return pResult;
|
|
}
|
|
|
|
INLINE Matrix4x4 Mat4x4RotationX(float angle)
|
|
{
|
|
Matrix4x4 matrix;
|
|
float num2 = cosf(angle);
|
|
float num = sinf(angle);
|
|
matrix.M11 = 1.0f;
|
|
matrix.M12 = 0.0f;
|
|
matrix.M13 = 0.0f;
|
|
matrix.M14 = 0.0f;
|
|
matrix.M21 = 0.0f;
|
|
matrix.M22 = num2;
|
|
matrix.M23 = num;
|
|
matrix.M24 = 0.0f;
|
|
matrix.M31 = 0.0f;
|
|
matrix.M32 = -num;
|
|
matrix.M33 = num2;
|
|
matrix.M34 = 0.0f;
|
|
matrix.M41 = 0.0f;
|
|
matrix.M42 = 0.0f;
|
|
matrix.M43 = 0.0f;
|
|
matrix.M44 = 1.0f;
|
|
return matrix;
|
|
}
|
|
|
|
INLINE void Mat4x4RotationX(float angle, Matrix4x4& matrix)
|
|
{
|
|
float num2 = cosf(angle);
|
|
float num = sinf(angle);
|
|
matrix.M11 = 1.0f;
|
|
matrix.M12 = 0.0f;
|
|
matrix.M13 = 0.0f;
|
|
matrix.M14 = 0.0f;
|
|
matrix.M21 = 0.0f;
|
|
matrix.M22 = num2;
|
|
matrix.M23 = num;
|
|
matrix.M24 = 0.0f;
|
|
matrix.M31 = 0.0f;
|
|
matrix.M32 = -num;
|
|
matrix.M33 = num2;
|
|
matrix.M34 = 0.0f;
|
|
matrix.M41 = 0.0f;
|
|
matrix.M42 = 0.0f;
|
|
matrix.M43 = 0.0f;
|
|
matrix.M44 = 1.0f;
|
|
}
|
|
|
|
INLINE Matrix4x4 Mat4x4RotationY(float angle)
|
|
{
|
|
Matrix4x4 matrix;
|
|
float num2 = cosf(angle);
|
|
float num = sinf(angle);
|
|
matrix.M11 = num2;
|
|
matrix.M12 = 0.0f;
|
|
matrix.M13 = -num;
|
|
matrix.M14 = 0.0f;
|
|
matrix.M21 = 0.0f;
|
|
matrix.M22 = 1.0f;
|
|
matrix.M23 = 0.0f;
|
|
matrix.M24 = 0.0f;
|
|
matrix.M31 = num;
|
|
matrix.M32 = 0.0f;
|
|
matrix.M33 = num2;
|
|
matrix.M34 = 0.0f;
|
|
matrix.M41 = 0.0f;
|
|
matrix.M42 = 0.0f;
|
|
matrix.M43 = 0.0f;
|
|
matrix.M44 = 1.0f;
|
|
return matrix;
|
|
}
|
|
|
|
INLINE void Mat4x4RotationY(float angle, Matrix4x4& matrix)
|
|
{
|
|
float num2 = cosf(angle);
|
|
float num = sinf(angle);
|
|
matrix.M11 = num2;
|
|
matrix.M12 = 0.0f;
|
|
matrix.M13 = -num;
|
|
matrix.M14 = 0.0f;
|
|
matrix.M21 = 0.0f;
|
|
matrix.M22 = 1.0f;
|
|
matrix.M23 = 0.0f;
|
|
matrix.M24 = 0.0f;
|
|
matrix.M31 = num;
|
|
matrix.M32 = 0.0f;
|
|
matrix.M33 = num2;
|
|
matrix.M34 = 0.0f;
|
|
matrix.M41 = 0.0f;
|
|
matrix.M42 = 0.0f;
|
|
matrix.M43 = 0.0f;
|
|
matrix.M44 = 1.0f;
|
|
}
|
|
|
|
INLINE Matrix4x4 Mat4x4RotationZ(float angle)
|
|
{
|
|
Matrix4x4 matrix;
|
|
float num2 = cosf(angle);
|
|
float num = sinf(angle);
|
|
matrix.M11 = num2;
|
|
matrix.M12 = num;
|
|
matrix.M13 = 0.0f;
|
|
matrix.M14 = 0.0f;
|
|
matrix.M21 = -num;
|
|
matrix.M22 = num2;
|
|
matrix.M23 = 0.0f;
|
|
matrix.M24 = 0.0f;
|
|
matrix.M31 = 0.0f;
|
|
matrix.M32 = 0.0f;
|
|
matrix.M33 = 1.0f;
|
|
matrix.M34 = 0.0f;
|
|
matrix.M41 = 0.0f;
|
|
matrix.M42 = 0.0f;
|
|
matrix.M43 = 0.0f;
|
|
matrix.M44 = 1.0f;
|
|
return matrix;
|
|
}
|
|
|
|
INLINE void Mat4x4RotationZ(float angle, Matrix4x4& matrix)
|
|
{
|
|
float num2 = cosf(angle);
|
|
float num = sinf(angle);
|
|
matrix.M11 = num2;
|
|
matrix.M12 = num;
|
|
matrix.M13 = 0.0f;
|
|
matrix.M14 = 0.0f;
|
|
matrix.M21 = -num;
|
|
matrix.M22 = num2;
|
|
matrix.M23 = 0.0f;
|
|
matrix.M24 = 0.0f;
|
|
matrix.M31 = 0.0f;
|
|
matrix.M32 = 0.0f;
|
|
matrix.M33 = 1.0f;
|
|
matrix.M34 = 0.0f;
|
|
matrix.M41 = 0.0f;
|
|
matrix.M42 = 0.0f;
|
|
matrix.M43 = 0.0f;
|
|
matrix.M44 = 1.0f;
|
|
}
|
|
|
|
INLINE void Mat4x4RotationQuaternion(const Quaternion& rotation, Matrix4x4& result)
|
|
{
|
|
float x = rotation.X;
|
|
float num9 = (x * x);
|
|
float y = rotation.Y;
|
|
float num8 = (y * y);
|
|
float z = rotation.Z;
|
|
float num7 = (z * z);
|
|
float num6 = rotation.Y * rotation.X;
|
|
float num5 = rotation.W * rotation.Z;
|
|
float num4 = rotation.Z * rotation.X;
|
|
float num3 = rotation.W * rotation.Y;
|
|
float num2 = rotation.Z * rotation.Y;
|
|
float num = rotation.W * rotation.X;
|
|
result.M11 = (1.0f - ((num7 + num8) * 2.0f));
|
|
result.M12 = ((num5 + num6) * 2.0f);
|
|
result.M13 = ((num4 - num3) * 2.0f);
|
|
result.M14 = 0.0f;
|
|
result.M21 = ((num6 - num5) * 2.0f);
|
|
result.M22 = (1.0f - ((num7 + num9) * 2.0f));
|
|
result.M23 = ((num + num2) * 2.0f);
|
|
result.M24 = 0.0f;
|
|
result.M31 = ((num3 + num4) * 2.0f);
|
|
result.M32 = ((num2 - num) * 2.0f);
|
|
result.M33 = (1.0f - ((num8 + num9) * 2.0f));
|
|
result.M34 = 0.0f;
|
|
result.M41 = 0.0f;
|
|
result.M42 = 0.0f;
|
|
result.M43 = 0.0f;
|
|
result.M44 = 1.0f;
|
|
}
|
|
|
|
INLINE void Mat4x4RotationYawPitchRoll(float yaw, float pitch, float roll, Matrix4x4& out)
|
|
{
|
|
Quaternion quaternion;
|
|
QuatRotationYawPitchRoll(yaw, pitch, roll, quaternion);
|
|
Mat4x4RotationQuaternion(quaternion, out);
|
|
}
|
|
|
|
INLINE void Mat4x4Scaling(float xScale, float yScale, float zScale, Matrix4x4& out)
|
|
{
|
|
out.M11 = xScale;
|
|
out.M12 = 0.0f;
|
|
out.M13 = 0.0f;
|
|
out.M14 = 0.0f;
|
|
out.M21 = 0.0f;
|
|
out.M22 = yScale;
|
|
out.M23 = 0.0f;
|
|
out.M24 = 0.0f;
|
|
out.M31 = 0.0f;
|
|
out.M32 = 0.0f;
|
|
out.M33 = zScale;
|
|
out.M34 = 0.0f;
|
|
out.M41 = 0.0f;
|
|
out.M42 = 0.0f;
|
|
out.M43 = 0.0f;
|
|
out.M44 = 1.0f;
|
|
}
|
|
|
|
INLINE void Mat4x4Scaling(Vec3f& amt, Matrix4x4& matrix)
|
|
{
|
|
matrix.M11 = amt.X;
|
|
matrix.M12 = 0.0f;
|
|
matrix.M13 = 0.0f;
|
|
matrix.M14 = 0.0f;
|
|
matrix.M21 = 0.0f;
|
|
matrix.M22 = amt.Y;
|
|
matrix.M23 = 0.0f;
|
|
matrix.M24 = 0.0f;
|
|
matrix.M31 = 0.0f;
|
|
matrix.M32 = 0.0f;
|
|
matrix.M33 = amt.Z;
|
|
matrix.M34 = 0.0f;
|
|
matrix.M41 = 0.0f;
|
|
matrix.M42 = 0.0f;
|
|
matrix.M43 = 0.0f;
|
|
matrix.M44 = 1.0f;
|
|
}
|
|
|
|
INLINE void Mat4x4Translation(const Vec3f& amount, Matrix4x4& matrix)
|
|
{
|
|
matrix.M11 = 1.0f;
|
|
matrix.M12 = 0.0f;
|
|
matrix.M13 = 0.0f;
|
|
matrix.M14 = 0.0f;
|
|
matrix.M21 = 0.0f;
|
|
matrix.M22 = 1.0f;
|
|
matrix.M23 = 0.0f;
|
|
matrix.M24 = 0.0f;
|
|
matrix.M31 = 0.0f;
|
|
matrix.M32 = 0.0f;
|
|
matrix.M33 = 1.0f;
|
|
matrix.M34 = 0.0f;
|
|
matrix.M41 = amount.X;
|
|
matrix.M42 = amount.Y;
|
|
matrix.M43 = amount.Z;
|
|
matrix.M44 = 1.0f;
|
|
}
|
|
|
|
INLINE void Mat4x4Translation(float _x, float _y, float _z, Matrix4x4& matrix)
|
|
{
|
|
matrix.M11 = 1.0f;
|
|
matrix.M12 = 0.0f;
|
|
matrix.M13 = 0.0f;
|
|
matrix.M14 = 0.0f;
|
|
matrix.M21 = 0.0f;
|
|
matrix.M22 = 1.0f;
|
|
matrix.M23 = 0.0f;
|
|
matrix.M24 = 0.0f;
|
|
matrix.M31 = 0.0f;
|
|
matrix.M32 = 0.0f;
|
|
matrix.M33 = 1.0f;
|
|
matrix.M34 = 0.0f;
|
|
matrix.M41 = _x;
|
|
matrix.M42 = _y;
|
|
matrix.M43 = _z;
|
|
matrix.M44 = 1.0f;
|
|
}
|
|
|
|
INLINE void Mat4x4Transpose(const Matrix4x4& src, Matrix4x4& dst)
|
|
{
|
|
Matrix4x4 tmp = src;
|
|
dst.M11 = tmp.M11;
|
|
dst.M12 = tmp.M21;
|
|
dst.M13 = tmp.M31;
|
|
dst.M14 = tmp.M41;
|
|
dst.M21 = tmp.M12;
|
|
dst.M22 = tmp.M22;
|
|
dst.M23 = tmp.M32;
|
|
dst.M24 = tmp.M42;
|
|
dst.M31 = tmp.M13;
|
|
dst.M32 = tmp.M23;
|
|
dst.M33 = tmp.M33;
|
|
dst.M34 = tmp.M43;
|
|
dst.M41 = tmp.M14;
|
|
dst.M42 = tmp.M24;
|
|
dst.M43 = tmp.M34;
|
|
dst.M44 = tmp.M44;
|
|
}
|
|
|
|
INLINE void Mat4x4Lerp(const Matrix4x4& start, const Matrix4x4& end, Matrix4x4& result, float amount)
|
|
{
|
|
result.M11 = ((end.M11 - start.M11) * amount) + start.M11;
|
|
result.M12 = ((end.M12 - start.M12) * amount) + start.M12;
|
|
result.M13 = ((end.M13 - start.M13) * amount) + start.M13;
|
|
result.M14 = ((end.M14 - start.M14) * amount) + start.M14;
|
|
result.M21 = ((end.M21 - start.M21) * amount) + start.M21;
|
|
result.M22 = ((end.M22 - start.M22) * amount) + start.M22;
|
|
result.M23 = ((end.M23 - start.M23) * amount) + start.M23;
|
|
result.M24 = ((end.M24 - start.M24) * amount) + start.M24;
|
|
result.M31 = ((end.M31 - start.M31) * amount) + start.M31;
|
|
result.M32 = ((end.M32 - start.M32) * amount) + start.M32;
|
|
result.M33 = ((end.M33 - start.M33) * amount) + start.M33;
|
|
result.M34 = ((end.M34 - start.M34) * amount) + start.M34;
|
|
result.M41 = ((end.M41 - start.M41) * amount) + start.M41;
|
|
result.M42 = ((end.M42 - start.M42) * amount) + start.M42;
|
|
result.M43 = ((end.M43 - start.M43) * amount) + start.M43;
|
|
result.M44 = ((end.M44 - start.M44) * amount) + start.M44;
|
|
}
|
|
|
|
INLINE void Mat4x4Billboard(const Vec3f& objectPosition, const Vec3f& cameraPosition, const Vec3f& cameraUpVector, const Vec3f& cameraForwardVector, Matrix4x4& matrix)
|
|
{
|
|
Vec3f eyeDir = objectPosition - cameraPosition;
|
|
Vec3f right = eyeDir;
|
|
Vec3f result;
|
|
Vec3f vector3;
|
|
|
|
float x = right.X;
|
|
float y = right.Y;
|
|
float z = right.Z;
|
|
|
|
float num = (((x * x) + (y * y)) + (z * z));
|
|
if (num < 9.99999974737875f)
|
|
{
|
|
right = -(cameraForwardVector);
|
|
}
|
|
else
|
|
{
|
|
right = (Vec3f)(eyeDir * ((1.0f / sqrtf(num))));
|
|
}
|
|
Vec3Cross(cameraUpVector, right, result);
|
|
Vec3Normalize(&result);
|
|
Vec3Cross(right, result, vector3);
|
|
|
|
matrix.M11 = vector3.X;
|
|
matrix.M12 = vector3.Y;
|
|
matrix.M13 = vector3.Z;
|
|
matrix.M14 = 0.0f;
|
|
matrix.M21 = result.X;
|
|
matrix.M22 = result.Y;
|
|
matrix.M23 = result.Z;
|
|
matrix.M24 = 0.0f;
|
|
matrix.M31 = right.X;
|
|
matrix.M32 = right.Y;
|
|
matrix.M33 = right.Z;
|
|
matrix.M34 = 0.0f;
|
|
matrix.M41 = objectPosition.X;
|
|
matrix.M42 = objectPosition.Y;
|
|
matrix.M43 = objectPosition.Z;
|
|
matrix.M44 = 1.0f;
|
|
}
|
|
|
|
INLINE void Mat4x4LookAtLH(const Vec3f& cameraPosition, const Vec3f& cameraTarget, const Vec3f& cameraUpVector, Matrix4x4& matrix)
|
|
{
|
|
Vec3f camDir = cameraTarget - cameraPosition;
|
|
Vec3Normalize(&camDir);
|
|
|
|
// Vec3f right = Vector3D.Normalize(Vector3D.Cross(cameraUpVector, camDir));
|
|
|
|
Vec3f right;
|
|
Vec3Cross(cameraUpVector, camDir, right);
|
|
Vec3Normalize(&right);
|
|
|
|
Vec3f up;
|
|
Vec3Cross(camDir, right, up);
|
|
|
|
matrix.M11 = right.X;
|
|
matrix.M12 = up.X;
|
|
matrix.M13 = camDir.X;
|
|
matrix.M14 = 0.0f;
|
|
matrix.M21 = right.Y;
|
|
matrix.M22 = up.Y;
|
|
matrix.M23 = camDir.Y;
|
|
matrix.M24 = 0.0f;
|
|
matrix.M31 = right.Z;
|
|
matrix.M32 = up.Z;
|
|
matrix.M33 = camDir.Z;
|
|
matrix.M34 = 0.0f;
|
|
matrix.M41 = -Vec3Dot(&right, &cameraPosition);
|
|
matrix.M42 = -Vec3Dot(&up, &cameraPosition);
|
|
matrix.M43 = -Vec3Dot(&camDir, &cameraPosition);
|
|
matrix.M44 = 1.0f;
|
|
}
|
|
|
|
INLINE void Mat4x4Decompose(Matrix4x4& matrix, Vec3f& position, Vec3f& scale, Quaternion& rot)
|
|
{
|
|
//compute scaling
|
|
Vec3f Row0(matrix.M11, matrix.M12, matrix.M13);
|
|
Vec3f Row1(matrix.M21, matrix.M22, matrix.M23);
|
|
Vec3f Row2(matrix.M31, matrix.M32, matrix.M33);
|
|
scale.X = Vec3Length(&Row0);
|
|
scale.Y = Vec3Length(&Row1);
|
|
scale.Z = Vec3Length(&Row2);
|
|
|
|
//translation
|
|
position.X = matrix.M41;
|
|
position.Y = matrix.M42;
|
|
position.Z = matrix.M43;
|
|
|
|
//rotation
|
|
QuatRotationMatrix(matrix,rot);
|
|
}
|
|
|
|
INLINE void Mat4x4PerspectiveFOV(float fieldOfView, float aspectRatio, float nearPlaneDistance, float farPlaneDistance, Matrix4x4& matrix)
|
|
{
|
|
/*if ((fieldOfView <= 0.0f) || (fieldOfView >= (2.0f * 3.1415926535f)))
|
|
{
|
|
throw new ArgumentOutOfRangeException("FieldOfView is not correct!");
|
|
}
|
|
if (nearPlaneDistance <= 0.0f)
|
|
{
|
|
throw new ArgumentOutOfRangeException("Near plane distance must be greater than zero");
|
|
}
|
|
if (farPlaneDistance <= 0.0f)
|
|
{
|
|
throw new ArgumentOutOfRangeException("Far plane distance must be greater than zero");
|
|
}
|
|
if (nearPlaneDistance >= farPlaneDistance)
|
|
{
|
|
throw new ArgumentOutOfRangeException("Distance to the far plane must be greater than distance to the near plane!");
|
|
}*/
|
|
float num = 1.0f / (tanf(fieldOfView * 0.5f));
|
|
float num9 = num / aspectRatio;
|
|
matrix.M11 = num9;
|
|
matrix.M12 = matrix.M13 = matrix.M14 = 0.0f;
|
|
matrix.M22 = num;
|
|
matrix.M21 = matrix.M23 = matrix.M24 = 0.0f;
|
|
matrix.M31 = matrix.M32 = 0.0f;
|
|
matrix.M33 = farPlaneDistance / (farPlaneDistance - nearPlaneDistance);
|
|
matrix.M34 = 1.0f;
|
|
matrix.M41 = matrix.M42 = matrix.M44 = 0.0f;
|
|
matrix.M43 = -nearPlaneDistance * farPlaneDistance / (farPlaneDistance - nearPlaneDistance);
|
|
}
|
|
//-----Quaternion-----
|
|
INLINE void QuatIdentity(Quaternion& pDst)
|
|
{
|
|
pDst.W = pDst.Y = pDst.Z = 0.0f;
|
|
pDst.X = 1.0f;
|
|
}
|
|
|
|
INLINE void QuatLerp(const Quaternion& quaternion1, const Quaternion& quaternion2, float amount, Quaternion& result)
|
|
{
|
|
float num = amount;
|
|
float num2 = 1.0f - num;
|
|
float num5 = (((quaternion1.X * quaternion2.X) + (quaternion1.Y * quaternion2.Y)) + (quaternion1.Z * quaternion2.Z)) + (quaternion1.W * quaternion2.W);
|
|
if (num5 >= 0.0f)
|
|
{
|
|
result.X = (num2 * quaternion1.X) + (num * quaternion2.X);
|
|
result.Y = (num2 * quaternion1.Y) + (num * quaternion2.Y);
|
|
result.Z = (num2 * quaternion1.Z) + (num * quaternion2.Z);
|
|
result.W = (num2 * quaternion1.W) + (num * quaternion2.W);
|
|
}
|
|
else
|
|
{
|
|
result.X = (num2 * quaternion1.X) - (num * quaternion2.X);
|
|
result.Y = (num2 * quaternion1.Y) - (num * quaternion2.Y);
|
|
result.Z = (num2 * quaternion1.Z) - (num * quaternion2.Z);
|
|
result.W = (num2 * quaternion1.W) - (num * quaternion2.W);
|
|
}
|
|
float num4 = (((result.X * result.X) + (result.Y * result.Y)) + (result.Z * result.Z)) + (result.W * result.W);
|
|
|
|
if(num4 <=0)
|
|
printf("OLOLOOOOOOOOOOOOOOOOO");
|
|
float num3 = 1.0f / sqrtf(num4);
|
|
result.X *= num3;
|
|
result.Y *= num3;
|
|
result.Z *= num3;
|
|
result.W *= num3;
|
|
}
|
|
|
|
INLINE void QuatSLerp( const Quaternion &from, const Quaternion &to, float t, Quaternion& result )
|
|
{
|
|
Quaternion temp;
|
|
float omega, cosom, sinom, scale0, scale1;
|
|
|
|
if ( t <= 0.0f )
|
|
{
|
|
result = from;
|
|
return;
|
|
}
|
|
|
|
if ( t >= 1.0f )
|
|
{
|
|
result = to;
|
|
return;
|
|
}
|
|
|
|
if ( from == to )
|
|
{
|
|
result = to;
|
|
return;
|
|
}
|
|
|
|
cosom = from.X * to.X + from.Y * to.Y + from.Z * to.Z + from.W * to.W;
|
|
if ( cosom < 0.0f )
|
|
{
|
|
temp = -to;
|
|
cosom = -cosom;
|
|
} else
|
|
{
|
|
temp = to;
|
|
}
|
|
|
|
if ( ( 1.0f - cosom ) > 1e-6f ) {
|
|
omega = acos( cosom );
|
|
sinom = 1.0f / sin( omega );
|
|
scale0 = sin( ( 1.0f - t ) * omega ) * sinom;
|
|
scale1 = sin( t * omega ) * sinom;
|
|
} else {
|
|
scale0 = 1.0f - t;
|
|
scale1 = t;
|
|
}
|
|
|
|
result = ( from * scale0) + ( temp * scale1);
|
|
}
|
|
|
|
INLINE float QuatLength(const Quaternion& src)
|
|
{
|
|
return sqrtf((src.X * src.X) + (src.Y * src.Y) + (src.Z * src.Z) + (src.W * src.W));
|
|
}
|
|
|
|
INLINE void QuatNormalize(Quaternion& src)
|
|
{
|
|
float num = (1.0f / QuatLength(src));
|
|
src.X *= num;
|
|
src.Y *= num;
|
|
src.Z *= num;
|
|
src.W *= num;
|
|
}
|
|
|
|
INLINE void QuatSetAxisAngle(const Vec3f& axis, float theta, Quaternion& dst)
|
|
{
|
|
dst.X = sinf(theta * 0.5f) * axis.X;
|
|
dst.Y = sinf(theta * 0.5f) * axis.Y;
|
|
dst.Z = sinf(theta * 0.5f) * axis.Z;
|
|
dst.W = cosf(theta * 0.5f);
|
|
}
|
|
|
|
INLINE void QuatInvert(Quaternion& src)
|
|
{
|
|
float num2 = (((src.X * src.X) + (src.Y * src.Y)) + (src.Z * src.Z)) + (src.W * src.W);
|
|
float num = 1.0f / num2;
|
|
src.X = -src.X * num;
|
|
src.Y = -src.Y * num;
|
|
src.Z = -src.Z * num;
|
|
src.W = src.W * num;
|
|
}
|
|
//-----Plane-----
|
|
INLINE bool PlaneIntersectLine(const Plane& plane, const Vec3f& start, const Vec3f& dir, float& dist)
|
|
{
|
|
dist = 0;
|
|
float num2 = ((plane.Normal.X * dir.X) + (plane.Normal.Y * dir.Y)) + (plane.Normal.Z * dir.Z);
|
|
if (fabs(num2) < 1E-05f)
|
|
{
|
|
return false;
|
|
}
|
|
float num3 = ((plane.Normal.X * start.X) + (plane.Normal.Y * start.Y)) + (plane.Normal.Z * start.Z);
|
|
float num = (-plane.D - num3) / num2;
|
|
if (num < 0.0f)
|
|
{
|
|
if (num < -1E-05f)
|
|
{
|
|
return false;
|
|
}
|
|
num = 0.0f;
|
|
}
|
|
dist = num;
|
|
return true;
|
|
}
|
|
INLINE void PlaneNormalize(Plane& plane)
|
|
{
|
|
float num2 = ((plane.Normal.X * plane.Normal.X) + (plane.Normal.Y * plane.Normal.Y)) + (plane.Normal.Z * plane.Normal.Z);
|
|
if (fabs(num2 - 1.0f) >= 1.192093E-07f)
|
|
{
|
|
float num = 1.0f / sqrtf(num2);
|
|
plane.Normal.X *= num;
|
|
plane.Normal.Y *= num;
|
|
plane.Normal.Z *= num;
|
|
plane.D *= num;
|
|
}
|
|
}
|
|
INLINE void PlaneTransform(Plane& plane, const Matrix4x4& matrix)
|
|
{
|
|
Matrix4x4 matrix2 = matrix;
|
|
matrix2 = Mat4x4Invert(&matrix2);
|
|
float x = plane.Normal.X;
|
|
float y = plane.Normal.Y;
|
|
float z = plane.Normal.Z;
|
|
float d = plane.D;
|
|
plane.Normal.X = (((x * matrix2.M11) + (y * matrix2.M12)) + (z * matrix2.M13)) + (d * matrix2.M14);
|
|
plane.Normal.Y = (((x * matrix2.M21) + (y * matrix2.M22)) + (z * matrix2.M23)) + (d * matrix2.M24);
|
|
plane.Normal.Z = (((x * matrix2.M31) + (y * matrix2.M32)) + (z * matrix2.M33)) + (d * matrix2.M34);
|
|
plane.D = (((x * matrix2.M41) + (y * matrix2.M42)) + (z * matrix2.M43)) + (d * matrix2.M44);
|
|
}
|
|
INLINE void PlaneTransform(const Plane& plane, const Matrix4x4& matrix, Plane& result)
|
|
{
|
|
Matrix4x4 matrix2 = matrix;
|
|
matrix2 = Mat4x4Invert(&matrix2);
|
|
float x = plane.Normal.X;
|
|
float y = plane.Normal.Y;
|
|
float z = plane.Normal.Z;
|
|
float d = plane.D;
|
|
result.Normal.X = (((x * matrix2.M11) + (y * matrix2.M12)) + (z * matrix2.M13)) + (d * matrix2.M14);
|
|
result.Normal.Y = (((x * matrix2.M21) + (y * matrix2.M22)) + (z * matrix2.M23)) + (d * matrix2.M24);
|
|
result.Normal.Z = (((x * matrix2.M31) + (y * matrix2.M32)) + (z * matrix2.M33)) + (d * matrix2.M34);
|
|
result.D = (((x * matrix2.M41) + (y * matrix2.M42)) + (z * matrix2.M43)) + (d * matrix2.M44);
|
|
}
|
|
INLINE float PlaneDot(const Plane& plane, const Vec4f& value)
|
|
{
|
|
return ((((plane.Normal.X * value.X) + (plane.Normal.Y * value.Y)) + (plane.Normal.Z * value.Z)) + (plane.D * value.W));
|
|
}
|
|
INLINE float PlaneDotCoordinate(const Plane& plane, const Vec3f& value)
|
|
{
|
|
return ((((plane.Normal.X * value.X) + (plane.Normal.Y * value.Y)) + (plane.Normal.Z * value.Z)) + plane.D);
|
|
}
|
|
|
|
INLINE float PlaneDotNormal(const Plane& plane, const Vec3f& value)
|
|
{
|
|
return (((plane.Normal.X * value.X) + (plane.Normal.Y * value.Y)) + (plane.Normal.Z * value.Z));
|
|
}
|
|
INLINE float PlaneDot(const Plane& plane, const Vec3f& point)
|
|
{
|
|
return ((((plane.Normal.Y * point.Y) + (plane.Normal.X * point.X)) + (plane.Normal.Z * point.Z)) + plane.D);
|
|
}
|
|
|
|
INLINE void Get4x4Offsets(int destTextureWidth, int destTextureHeight, Vec4f* output)
|
|
{
|
|
float tu = 1.0f / destTextureWidth;
|
|
float tv = 1.0f / destTextureHeight;
|
|
|
|
int index = 0;
|
|
for (int x = 0; x < 4; x++)
|
|
{
|
|
for (int y = 0; y < 4; y++)
|
|
{
|
|
output[index].X = (x - 1.5f) * tu;
|
|
output[index].Y = (y - 1.5f) * tv;
|
|
|
|
index++;
|
|
}
|
|
}
|
|
}
|
|
INLINE void Get3x3Offsets(int destTextureWidth, int destTextureHeight, Vec4f* output)
|
|
{
|
|
float sampleOffset = 1.0f;
|
|
float tu = 1.0f / destTextureWidth * sampleOffset;
|
|
float tv = 1.0f / destTextureHeight * sampleOffset;
|
|
|
|
int index = 0;
|
|
for (int x = 0; x < 3; x++)
|
|
{
|
|
for (int y = 0; y < 3; y++)
|
|
{
|
|
output[index].X = (x - 1.5f) * tu;
|
|
output[index].Y = (y - 1.5f) * tv;
|
|
|
|
index++;
|
|
}
|
|
}
|
|
}
|
|
INLINE void Get2x2Offsets(int destTextureWidth, int destTextureHeight, Vec4f* output)
|
|
{
|
|
float sampleOffset = 1.0f;
|
|
float tu = 1.0f / destTextureWidth * sampleOffset;
|
|
float tv = 1.0f / destTextureHeight * sampleOffset;
|
|
|
|
//Vector4D[] offsets = new Vector4D[4];
|
|
int index = 0;
|
|
for (int x = 0; x < 2; x++)
|
|
{
|
|
for (int y = 0; y < 2; y++)
|
|
{
|
|
output[index].X = (x - 0.5f) * tu;
|
|
output[index].Y = (y - 0.5f) * tv;
|
|
|
|
index++;
|
|
}
|
|
}
|
|
}
|
|
INLINE void GetBlurWeightedOffsets(float numSamples, int destTextureDimensionLength, Vec4f* output)
|
|
{
|
|
float tuv = 1.0f / destTextureDimensionLength;
|
|
|
|
float offset = -(numSamples - 1.0f) / 2.0f;
|
|
for (int i = 0; i < numSamples; i++)
|
|
{
|
|
output[i].X = offset * tuv;
|
|
offset++;
|
|
float x = 2.0f * i / (numSamples - 1.0f) - 1.0f;
|
|
output[i].Y = -0.25f * x * x + 0.225f;
|
|
}
|
|
}
|
|
}
|
|
#endif // LifeMath_h__
|