|
|
/*
|
|
|
* Copyright (c) Hisilicon Technologies Co., Ltd. 2021-2023. All rights reserved.
|
|
|
* Description: Math module
|
|
|
* Author: Hisilicon
|
|
|
* Created: 2021.04.16
|
|
|
*/
|
|
|
|
|
|
#ifndef IMAGEKIT_IMAGE_MATH_H
|
|
|
#define IMAGEKIT_IMAGE_MATH_H
|
|
|
|
|
|
#include <math.h>
|
|
|
#include <stdint.h>
|
|
|
#include <stdlib.h>
|
|
|
#include <stdio.h>
|
|
|
#include <string.h>
|
|
|
#include <float.h>
|
|
|
|
|
|
/* -----------------------------------------------------------------------------------
|
|
|
* ***** IMAGE_MATH_ALIGNAS
|
|
|
*/
|
|
|
#if !defined(IMAGE_MATH_ALIGNAS)
|
|
|
#if defined(__GNUC__) || defined(__clang__)
|
|
|
#define IMAGE_MATH_ALIGNAS(n) __attribute__((aligned(n)))
|
|
|
#elif defined(__CC_ARM)
|
|
|
#define IMAGE_MATH_ALIGNAS(n) __align(n)
|
|
|
#else
|
|
|
#error Need to define IMAGE_MATH_ALIGNAS
|
|
|
#endif
|
|
|
#endif
|
|
|
|
|
|
/* -------------------------------------------------------------------------------------
|
|
|
* ***** IMAGE_MATH_ASSERT
|
|
|
*
|
|
|
* Independent debug break implementation for IMAGE_Math.h.
|
|
|
*/
|
|
|
#if !defined(IMAGE_MATH_DEBUG_BREAK)
|
|
|
#if defined(_DEBUG)
|
|
|
#define IMAGE_MATH_DEBUG_BREAK __builtin_trap()
|
|
|
#else
|
|
|
#define IMAGE_MATH_DEBUG_BREAK ((void)0)
|
|
|
#endif
|
|
|
#endif
|
|
|
|
|
|
|
|
|
/* -------------------------------------------------------------------------------------
|
|
|
* ***** IMAGE_MATH_ASSERT
|
|
|
*
|
|
|
* Independent IMAGE_MATH_ASSERT implementation for IMAGE_Math.h.
|
|
|
*/
|
|
|
#if !defined(IMAGE_MATH_ASSERT)
|
|
|
#if defined(_DEBUG)
|
|
|
#define IMAGE_MATH_ASSERT(p) \
|
|
|
if (!(p)) { \
|
|
|
IMAGE_MATH_DEBUG_BREAK; \
|
|
|
}
|
|
|
#else
|
|
|
#define IMAGE_MATH_ASSERT(p) ((void)0)
|
|
|
#endif
|
|
|
#endif
|
|
|
|
|
|
|
|
|
/* -------------------------------------------------------------------------------------
|
|
|
* ***** IMAGE_MATH_STATIC_ASSERT
|
|
|
*
|
|
|
* Independent IMAGE_MATH_ASSERT implementation for IMAGE_Math.h.
|
|
|
*/
|
|
|
#if !defined(IMAGE_MATH_STATIC_ASSERT)
|
|
|
#if defined(__cplusplus) && ((defined(_MSC_VER) && (defined(_MSC_VER) >= 1600)) || \
|
|
|
defined(__GXX_EXPERIMENTAL_CXX0X__) || (__cplusplus >= 201103L))
|
|
|
#define IMAGE_MATH_STATIC_ASSERT static_assert
|
|
|
#else
|
|
|
#if !defined(IMAGE_SA_UNUSED)
|
|
|
#if defined(__GNUC__) || defined(__clang__)
|
|
|
#define IMAGE_SA_UNUSED __attribute__((unused))
|
|
|
#else
|
|
|
#define IMAGE_SA_UNUSED
|
|
|
#endif
|
|
|
#define IMAGE_SA_PASTE(a, b) a##b
|
|
|
#define IMAGE_SA_HELP(a, b) IMAGE_SA_PASTE(a, b)
|
|
|
#endif
|
|
|
|
|
|
#define IMAGE_MATH_STATIC_ASSERT(expression, msg) \
|
|
|
typedef char IMAGE_SA_HELP(compileTimeAssert, __LINE__)[((expression) != 0) ? 1 : -1] IMAGE_SA_UNUSED
|
|
|
#endif
|
|
|
#endif
|
|
|
|
|
|
namespace PhoenixImage {
|
|
|
/* -----------------------------------------------------------------------------------
|
|
|
* ***** Simple Math Structures
|
|
|
*
|
|
|
*/
|
|
|
|
|
|
/* A 2D vector with float components. */
|
|
|
typedef struct IMAGE_MATH_ALIGNAS(4) ImageVector2f_
|
|
|
{
|
|
|
float x, y;
|
|
|
} ImageVector2f;
|
|
|
|
|
|
/* A 4x4 matrix with float elements. */
|
|
|
typedef struct IMAGE_MATH_ALIGNAS(4) ImageMatrix4f_ {
|
|
|
float m[4][4];
|
|
|
} ImageMatrix4f;
|
|
|
|
|
|
typedef struct IMAGE_MATH_ALIGNAS(4) ImageMatrix4d_ {
|
|
|
double m[4][4];
|
|
|
} ImageMatrix4d;
|
|
|
|
|
|
|
|
|
/* -------------------------------------------------------------------------------------
|
|
|
* ***** Constants for 3D world/axis definitions.
|
|
|
*
|
|
|
* Definitions of axes for coordinate and rotation conversions.
|
|
|
*/
|
|
|
enum Axis {
|
|
|
AXIS_X = 0,
|
|
|
AXIS_Y = 1,
|
|
|
AXIS_Z = 2
|
|
|
};
|
|
|
|
|
|
/* RotateDirection describes the rotation direction around an axis, interpreted as follows:
|
|
|
* CW - Clockwise while looking "down" from positive axis towards the origin.
|
|
|
* CCW - Counter-clockwise while looking from the positive axis towards the origin,
|
|
|
* which is in the negative axis direction.
|
|
|
* CCW is the default for the RHS coordinate system. Oculus standard RHS coordinate
|
|
|
* system defines Y up, X right, and Z back (pointing out from the screen). In this
|
|
|
* system ROTATE_CCW around Z will specifies counter-clockwise rotation in XY plane.
|
|
|
*/
|
|
|
enum RotateDirection {
|
|
|
ROTATE_CCW = 1,
|
|
|
ROTATE_CW = -1
|
|
|
};
|
|
|
|
|
|
/* Constants for right handed and left handed coordinate systems */
|
|
|
enum HandedSystem {
|
|
|
HANDED_R = 1,
|
|
|
HANDED_L = -1
|
|
|
};
|
|
|
|
|
|
/* AxisDirection describes which way the coordinate axis points. Used by WorldAxes. */
|
|
|
enum AxisDirection {
|
|
|
AXIS_UP = 2,
|
|
|
AXIS_DOWN = -2,
|
|
|
AXIS_RIGHT = 1,
|
|
|
AXIS_LEFT = -1,
|
|
|
AXIS_IN = 3,
|
|
|
AXIS_OUT = -3
|
|
|
};
|
|
|
|
|
|
struct WorldAxes {
|
|
|
AxisDirection axisX, axisY, axisZ;
|
|
|
|
|
|
WorldAxes(AxisDirection x, AxisDirection y, AxisDirection z) : axisX(x), axisY(y), axisZ(z)
|
|
|
{
|
|
|
IMAGE_MATH_ASSERT(abs(x) != abs(y) && abs(y) != abs(z) && abs(z) != abs(x));
|
|
|
}
|
|
|
};
|
|
|
|
|
|
/* Forward-declare our templates. */
|
|
|
template<class T> class Vector2;
|
|
|
template<class T> class Matrix4;
|
|
|
|
|
|
/* CompatibleTypes::Type is used to lookup a compatible C-version of a C++ class. */
|
|
|
template<class C> struct CompatibleTypes {
|
|
|
// Declaration here seems necessary for MSVC; specializations are
|
|
|
// used instead.
|
|
|
typedef struct {} Type;
|
|
|
};
|
|
|
|
|
|
// Specializations providing CompatibleTypes::Type value.
|
|
|
template<> struct CompatibleTypes<Vector2<float>> { typedef ImageVector2f Type;};
|
|
|
template<> struct CompatibleTypes<Matrix4<float>> { typedef ImageMatrix4f Type;};
|
|
|
template<> struct CompatibleTypes<Matrix4<double>> { typedef ImageMatrix4d Type;};
|
|
|
|
|
|
/* ------------------------------------------------------------------------------------//
|
|
|
* ***** Math
|
|
|
*
|
|
|
* Math class contains constants and functions. This class is a template specialized
|
|
|
* per type, with Math<float> and Math<double> being distinct.
|
|
|
*/
|
|
|
template<class T> class Math {
|
|
|
public:
|
|
|
/* By default, support explicit conversion to float. This allows Vector2<int> to
|
|
|
* compile, for example.
|
|
|
*/
|
|
|
typedef float OtherFloatType;
|
|
|
|
|
|
static int Tolerance()
|
|
|
{
|
|
|
return 0;
|
|
|
} /* Default value so integer types compile */
|
|
|
};
|
|
|
|
|
|
/* ------------------------------------------------------------------------------------//
|
|
|
* ***** double constants
|
|
|
*/
|
|
|
#define MATH_DOUBLE_PI 3.14159265358979323846
|
|
|
#define MATH_DOUBLE_PIOVER2 (0.5 * MATH_DOUBLE_PI)
|
|
|
|
|
|
/* ------------------------------------------------------------------------------------//
|
|
|
* ***** float constants
|
|
|
*/
|
|
|
#define MATH_FLOAT_PI float(MATH_DOUBLE_PI)
|
|
|
|
|
|
/* -------------------------------------------------------------------------------------
|
|
|
* ***** Vector2<>
|
|
|
*
|
|
|
* Vector2f (Vector2d) represents a 2-dimensional vector or point in space,
|
|
|
* consisting of coordinates x and y
|
|
|
*/
|
|
|
|
|
|
template<class T>
|
|
|
class Vector2 {
|
|
|
public:
|
|
|
typedef T ElementType;
|
|
|
static const size_t ElementCount = 2;
|
|
|
|
|
|
T x, y;
|
|
|
|
|
|
Vector2() : x(0), y(0) { }
|
|
|
Vector2(T x_, T y_) : x(x_), y(y_) { }
|
|
|
explicit Vector2(T s) : x(s), y(s) { }
|
|
|
|
|
|
// C-interop support.
|
|
|
typedef typename CompatibleTypes<Vector2<T> >::Type CompatibleType;
|
|
|
|
|
|
Vector2(const CompatibleType& s) : x(s.x), y(s.y) { }
|
|
|
|
|
|
operator const CompatibleType& () const
|
|
|
{
|
|
|
IMAGE_MATH_STATIC_ASSERT(sizeof(Vector2<T>) == sizeof(CompatibleType), "sizeof(Vector2<T>) failure");
|
|
|
return reinterpret_cast<const CompatibleType&>(*this);
|
|
|
}
|
|
|
|
|
|
bool operator== (const Vector2& b) const { return x == b.x && y == b.y; }
|
|
|
bool operator!= (const Vector2& b) const { return x != b.x || y != b.y; }
|
|
|
|
|
|
Vector2 operator+ (const Vector2& b) const
|
|
|
{
|
|
|
return Vector2(x + b.x, y + b.y);
|
|
|
}
|
|
|
|
|
|
Vector2& operator+= (const Vector2& b)
|
|
|
{
|
|
|
x += b.x;
|
|
|
y += b.y;
|
|
|
return *this;
|
|
|
}
|
|
|
|
|
|
Vector2 operator- (const Vector2& b) const
|
|
|
{
|
|
|
return Vector2(x - b.x, y - b.y);
|
|
|
}
|
|
|
|
|
|
Vector2& operator-= (const Vector2& b)
|
|
|
{
|
|
|
x -= b.x;
|
|
|
y -= b.y;
|
|
|
return *this;
|
|
|
}
|
|
|
|
|
|
Vector2 operator* (const Vector2& b) const
|
|
|
{
|
|
|
return Vector2(x * b.x, y * b.y);
|
|
|
}
|
|
|
|
|
|
Vector2& operator*= (const Vector2& b)
|
|
|
{
|
|
|
x *= b.x;
|
|
|
y *= b.y;
|
|
|
return *this;
|
|
|
}
|
|
|
|
|
|
Vector2 operator/ (const Vector2& b) const
|
|
|
{
|
|
|
return Vector2(x / b.x, y / b.y);
|
|
|
}
|
|
|
|
|
|
Vector2& operator/= (const Vector2& b)
|
|
|
{
|
|
|
x /= b.x;
|
|
|
y /= b.y;
|
|
|
return *this;
|
|
|
}
|
|
|
|
|
|
Vector2 operator- () const
|
|
|
{
|
|
|
return Vector2(-x, -y);
|
|
|
}
|
|
|
|
|
|
// Scalar multiplication/division scales vector.
|
|
|
Vector2 operator* (T s) const
|
|
|
{
|
|
|
return Vector2(x*s, y*s);
|
|
|
}
|
|
|
|
|
|
friend Vector2 operator* (T s, const Vector2& b)
|
|
|
{
|
|
|
return b * s;
|
|
|
}
|
|
|
|
|
|
Vector2& operator*= (T s)
|
|
|
{
|
|
|
x *= s;
|
|
|
y *= s;
|
|
|
return *this;
|
|
|
}
|
|
|
|
|
|
Vector2 operator/ (T s) const
|
|
|
{
|
|
|
T rcp = T(1)/s;
|
|
|
return Vector2(x*rcp, y*rcp);
|
|
|
}
|
|
|
Vector2& operator/= (T s)
|
|
|
{
|
|
|
T rcp = T(1)/s;
|
|
|
x *= rcp;
|
|
|
y *= rcp;
|
|
|
return *this;
|
|
|
}
|
|
|
|
|
|
// Access element by index
|
|
|
T& operator[] (int idx)
|
|
|
{
|
|
|
IMAGE_MATH_ASSERT(0 <= idx && idx < ElementCount);
|
|
|
return *(&x + idx);
|
|
|
}
|
|
|
|
|
|
// Dot product
|
|
|
// Used to calculate angle q between two vectors among other things,
|
|
|
// as (A dot B) = |a||b|cos(q).
|
|
|
T Dot(const Vector2& b) const
|
|
|
{
|
|
|
return x * b.x + y * b.y;
|
|
|
}
|
|
|
|
|
|
// Linearly interpolates from this vector to another.
|
|
|
// Factor should be between 0.0 and 1.0, with 0 giving full value to this.
|
|
|
Vector2 Lerp(const Vector2& b, T f) const
|
|
|
{
|
|
|
return *this * (T(1) - f) + b * f;
|
|
|
}
|
|
|
};
|
|
|
|
|
|
typedef Vector2<float> Vector2f;
|
|
|
typedef Vector2<double> Vector2d;
|
|
|
|
|
|
/* -------------------------------------------------------------------------------------
|
|
|
* ***** Matrix4
|
|
|
*
|
|
|
* Matrix4 is a 4x4 matrix used for 3d transformations and projections.
|
|
|
* Translation stored in the last column.
|
|
|
* The matrix is stored in row-major order in memory, meaning that values
|
|
|
* of the first row are stored before the next one.
|
|
|
*
|
|
|
* The arrangement of the matrix is chosen to be in Right-Handed
|
|
|
* coordinate system and counterclockwise rotations when looking down
|
|
|
* the axis
|
|
|
*
|
|
|
* Transformation Order:
|
|
|
* - Transformations are applied from right to left, so the expression
|
|
|
* M1 * M2 * M3 * V means that the vector V is transformed by M3 first,
|
|
|
* followed by M2 and M1.
|
|
|
*
|
|
|
* Coordinate system: Right Handed
|
|
|
*
|
|
|
* Rotations: Counterclockwise when looking down the axis. All angles are in radians.
|
|
|
*
|
|
|
* | sx 01 02 tx | // First column (sx, 10, 20): Axis X basis vector.
|
|
|
* | 10 sy 12 ty | // Second column (01, sy, 21): Axis Y basis vector.
|
|
|
* | 20 21 sz tz | // Third columnt (02, 12, sz): Axis Z basis vector.
|
|
|
* | 30 31 32 33 |
|
|
|
*
|
|
|
* The basis vectors are first three columns.
|
|
|
*/
|
|
|
template<class T> class Matrix4 {
|
|
|
public:
|
|
|
typedef T ElementType;
|
|
|
static const size_t dimension = 4;
|
|
|
|
|
|
T A[4][4];
|
|
|
|
|
|
enum NoInitType { NoInit};
|
|
|
|
|
|
/* Construct with no memory initialization. */
|
|
|
Matrix4(NoInitType) {}
|
|
|
|
|
|
/* By default, we construct identity matrix. */
|
|
|
Matrix4()
|
|
|
{
|
|
|
A[0][0] = T(1);
|
|
|
A[1][1] = T(1);
|
|
|
A[2][2] = T(1);
|
|
|
A[3][3] = T(1);
|
|
|
|
|
|
A[0][1] = T(0);
|
|
|
A[1][0] = T(0);
|
|
|
A[2][3] = T(0);
|
|
|
A[3][1] = T(0);
|
|
|
|
|
|
A[0][2] = T(0);
|
|
|
A[1][2] = T(0);
|
|
|
A[2][0] = T(0);
|
|
|
A[3][2] = T(0);
|
|
|
|
|
|
A[0][3] = T(0);
|
|
|
A[1][3] = T(0);
|
|
|
A[2][1] = T(0);
|
|
|
A[3][0] = T(0);
|
|
|
}
|
|
|
|
|
|
Matrix4(T m11, T m12, T m13, T m14,
|
|
|
T m21, T m22, T m23, T m24,
|
|
|
T m31, T m32, T m33, T m34,
|
|
|
T m41, T m42, T m43, T m44)
|
|
|
{
|
|
|
A[0][0] = m11;
|
|
|
A[0][1] = m12;
|
|
|
A[0][2] = m13;
|
|
|
A[0][3] = m14;
|
|
|
|
|
|
A[1][0] = m21;
|
|
|
A[1][1] = m22;
|
|
|
A[1][2] = m23;
|
|
|
A[1][3] = m24;
|
|
|
|
|
|
A[2][0] = m31;
|
|
|
A[2][1] = m32;
|
|
|
A[2][2] = m33;
|
|
|
A[2][3] = m34;
|
|
|
|
|
|
A[3][0] = m41;
|
|
|
A[3][1] = m42;
|
|
|
A[3][2] = m43;
|
|
|
A[3][3] = m44;
|
|
|
}
|
|
|
|
|
|
Matrix4(T m11, T m12, T m13, T m21, T m22, T m23, T m31, T m32, T m33)
|
|
|
{
|
|
|
A[0][0] = m11;
|
|
|
A[0][1] = m12;
|
|
|
A[0][2] = m13;
|
|
|
A[0][3] = T(0);
|
|
|
|
|
|
A[1][0] = m21;
|
|
|
A[1][1] = m22;
|
|
|
A[1][2] = m23;
|
|
|
A[1][3] = T(0);
|
|
|
|
|
|
A[2][0] = m31;
|
|
|
A[2][1] = m32;
|
|
|
A[2][2] = m33;
|
|
|
A[2][3] = T(0);
|
|
|
|
|
|
A[3][0] = T(0);
|
|
|
A[3][1] = T(0);
|
|
|
A[3][2] = T(0);
|
|
|
A[3][3] = T(1);
|
|
|
}
|
|
|
|
|
|
/* C-interop support. */
|
|
|
Matrix4(const typename CompatibleTypes<Matrix4<T>>::Type &s)
|
|
|
{
|
|
|
IMAGE_MATH_STATIC_ASSERT(sizeof(s) == sizeof(Matrix4), "sizeof(s) == sizeof(Matrix4)");
|
|
|
for (int x = 0; x < 4; x++) /* 4 * 4 matrix */
|
|
|
for (int y = 0; y < 4; y++) /* 4 * 4 matrix */
|
|
|
A[x][y] = (T)s.m[x][y];
|
|
|
}
|
|
|
|
|
|
operator typename CompatibleTypes<Matrix4<T>>::Type() const
|
|
|
{
|
|
|
typename CompatibleTypes<Matrix4<T>>::Type result;
|
|
|
IMAGE_MATH_STATIC_ASSERT(sizeof(result) == sizeof(Matrix4), "sizeof(result) == sizeof(Matrix4)");
|
|
|
for (int x = 0; x < 4; x++) /* 4 * 4 matrix */
|
|
|
for (int y = 0; y < 4; y++) /* 4 * 4 matrix */
|
|
|
result.m[x][y] = (T)A[x][y];
|
|
|
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
static Matrix4 Identity()
|
|
|
{
|
|
|
return Matrix4();
|
|
|
}
|
|
|
|
|
|
void SetIdentity()
|
|
|
{
|
|
|
A[0][0] = T(1);
|
|
|
A[1][1] = T(1);
|
|
|
A[2][2] = T(1);
|
|
|
A[3][3] = T(1);
|
|
|
|
|
|
A[0][1] = T(0);
|
|
|
A[1][0] = T(0);
|
|
|
A[2][3] = T(0);
|
|
|
A[3][1] = T(0);
|
|
|
|
|
|
A[0][2] = T(0);
|
|
|
A[1][2] = T(0);
|
|
|
A[2][0] = T(0);
|
|
|
A[3][2] = T(0);
|
|
|
|
|
|
A[0][3] = T(0);
|
|
|
A[1][3] = T(0);
|
|
|
A[2][1] = T(0);
|
|
|
A[3][0] = T(0);
|
|
|
}
|
|
|
|
|
|
bool operator == (const Matrix4 &b) const
|
|
|
{
|
|
|
bool isEqual = true;
|
|
|
for (int x = 0; x < 4; x++) /* 4 * 4 matrix */
|
|
|
for (int y = 0; y < 4; y++) /* 4 * 4 matrix */
|
|
|
isEqual &= (A[x][y] == b.A[x][y]);
|
|
|
|
|
|
return isEqual;
|
|
|
}
|
|
|
|
|
|
Matrix4 operator + (const Matrix4 &b) const
|
|
|
{
|
|
|
Matrix4 result(*this);
|
|
|
result += b;
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
Matrix4 &operator += (const Matrix4 &b)
|
|
|
{
|
|
|
for (int x = 0; x < 4; x++) /* 4 * 4 matrix */
|
|
|
for (int y = 0; y < 4; y++) /* 4 * 4 matrix */
|
|
|
A[x][y] += b.A[x][y];
|
|
|
return *this;
|
|
|
}
|
|
|
|
|
|
Matrix4 operator - (const Matrix4 &b) const
|
|
|
{
|
|
|
Matrix4 result(*this);
|
|
|
result -= b;
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
Matrix4 &operator -= (const Matrix4 &b)
|
|
|
{
|
|
|
for (int x = 0; x < 4; x++) /* 4 * 4 matrix */
|
|
|
for (int y = 0; y < 4; y++) /* 4 * 4 matrix */
|
|
|
A[x][y] -= b.A[x][y];
|
|
|
return *this;
|
|
|
}
|
|
|
|
|
|
// Multiplies two matrices into destination with minimum copying.
|
|
|
static Matrix4 &Multiply(Matrix4 *d, const Matrix4 &a, const Matrix4 &b)
|
|
|
{
|
|
|
IMAGE_MATH_ASSERT((d != &a) && (d != &b));
|
|
|
int i = 0;
|
|
|
do {
|
|
|
d->A[i][0] = a.A[i][0] * b.A[0][0] + a.A[i][1] * b.A[1][0] + a.A[i][2] * b.A[2][0] + a.A[i][3] * b.A[3][0];
|
|
|
d->A[i][1] = a.A[i][0] * b.A[0][1] + a.A[i][1] * b.A[1][1] + a.A[i][2] * b.A[2][1] + a.A[i][3] * b.A[3][1];
|
|
|
d->A[i][2] = a.A[i][0] * b.A[0][2] + a.A[i][1] * b.A[1][2] + a.A[i][2] * b.A[2][2] + a.A[i][3] * b.A[3][2];
|
|
|
d->A[i][3] = a.A[i][0] * b.A[0][3] + a.A[i][1] * b.A[1][3] + a.A[i][2] * b.A[2][3] + a.A[i][3] * b.A[3][3];
|
|
|
} while ((++i) < 4); /* 4 * 4 matrix */
|
|
|
|
|
|
return *d;
|
|
|
}
|
|
|
|
|
|
Matrix4 operator*(const Matrix4 &b) const
|
|
|
{
|
|
|
Matrix4 result(Matrix4::NoInit);
|
|
|
Multiply(&result, *this, b);
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
Matrix4 &operator *= (const Matrix4 &b)
|
|
|
{
|
|
|
return Multiply(this, Matrix4(*this), b);
|
|
|
}
|
|
|
|
|
|
Matrix4 operator*(T s) const
|
|
|
{
|
|
|
Matrix4 result(*this);
|
|
|
result *= s;
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
Matrix4 &operator *= (T s)
|
|
|
{
|
|
|
for (int x = 0; x < 4; x++) /* 4 * 4 matrix */
|
|
|
for (int y = 0; y < 4; y++) /* 4 * 4 matrix */
|
|
|
A[x][y] *= s;
|
|
|
return *this;
|
|
|
}
|
|
|
|
|
|
Matrix4 operator / (T s) const
|
|
|
{
|
|
|
Matrix4 result(*this);
|
|
|
result /= s;
|
|
|
return result;
|
|
|
}
|
|
|
|
|
|
Matrix4 &operator /= (T s)
|
|
|
{
|
|
|
for (int x = 0; x < 4; x++) /* 4 * 4 matrix */
|
|
|
for (int y = 0; y < 4; y++) /* 4 * 4 matrix */
|
|
|
A[x][y] /= s;
|
|
|
return *this;
|
|
|
}
|
|
|
|
|
|
Matrix4 Transposed() const
|
|
|
{
|
|
|
return Matrix4(A[0][0], A[1][0], A[2][0], A[3][0], A[0][1], A[1][1], A[2][1], A[3][1], A[0][2], A[1][2],
|
|
|
A[2][2], A[3][2], A[0][3], A[1][3], A[2][3], A[3][3]);
|
|
|
}
|
|
|
|
|
|
void Transpose()
|
|
|
{
|
|
|
*this = Transposed();
|
|
|
}
|
|
|
|
|
|
T SubDet(const size_t *rows, const size_t *cols) const
|
|
|
{
|
|
|
return A[rows[0]][cols[0]] *
|
|
|
(A[rows[1]][cols[1]] * A[rows[2]][cols[2]] - A[rows[1]][cols[2]] * A[rows[2]][cols[1]]) -
|
|
|
A[rows[0]][cols[1]] *
|
|
|
(A[rows[1]][cols[0]] * A[rows[2]][cols[2]] - A[rows[1]][cols[2]] * A[rows[2]][cols[0]]) +
|
|
|
A[rows[0]][cols[2]] *
|
|
|
(A[rows[1]][cols[0]] * A[rows[2]][cols[1]] - A[rows[1]][cols[1]] * A[rows[2]][cols[0]]);
|
|
|
}
|
|
|
|
|
|
T Cofactor(size_t I, size_t J) const
|
|
|
{
|
|
|
const size_t indices[4][3] = {{1, 2, 3}, {0, 2, 3}, {0, 1, 3}, {0, 1, 2}};
|
|
|
return ((I + J) & 1) ? -SubDet(indices[I], indices[J]) : SubDet(indices[I], indices[J]);
|
|
|
}
|
|
|
|
|
|
T Determinant() const
|
|
|
{
|
|
|
return A[0][0] * Cofactor(0, 0) + A[0][1] * Cofactor(0, 1) + A[0][2] * Cofactor(0, 2) +
|
|
|
A[0][3] * Cofactor(0, 3);
|
|
|
}
|
|
|
|
|
|
Matrix4 Adjugated() const
|
|
|
{
|
|
|
return Matrix4(Cofactor(0, 0), Cofactor(1, 0), Cofactor(2, 0), Cofactor(3, 0), Cofactor(0, 1), Cofactor(1, 1),
|
|
|
Cofactor(2, 1), Cofactor(3, 1), Cofactor(0, 2), Cofactor(1, 2), Cofactor(2, 2), Cofactor(3, 2),
|
|
|
Cofactor(0, 3), Cofactor(1, 3), Cofactor(2, 3), Cofactor(3, 3));
|
|
|
}
|
|
|
|
|
|
Matrix4 Inverted() const
|
|
|
{
|
|
|
T det = Determinant();
|
|
|
IMAGE_MATH_ASSERT(det != 0);
|
|
|
return Adjugated() * (T(1) / det);
|
|
|
}
|
|
|
|
|
|
void Invert()
|
|
|
{
|
|
|
*this = Inverted();
|
|
|
}
|
|
|
|
|
|
/* Matrix to Euler Angles conversion
|
|
|
* a,b,c, are the YawPitchRoll angles to be returned
|
|
|
* rotation a around axis A1
|
|
|
* is followed by rotation b around axis A2
|
|
|
* is followed by rotation c around axis A3
|
|
|
* rotations are CCW or CW (D) in LH or RH coordinate system (S)
|
|
|
*/
|
|
|
template<Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S> void ToEulerAngles(T *a, T *b, T *c) const
|
|
|
{
|
|
|
IMAGE_MATH_STATIC_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3), "(A1 != A2) && (A2 != A3) && (A1 != A3)");
|
|
|
|
|
|
T psign = T(-1);
|
|
|
if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) /* average value for 3: A1<41><31>A2<41><32>A3 */
|
|
|
psign = T(1);
|
|
|
|
|
|
T pm = psign * A[A1][A3];
|
|
|
T singularityRadius = Math<T>::SingularityRadius();
|
|
|
if (pm < T(-1) + singularityRadius) { // South pole singularity
|
|
|
*a = T(0);
|
|
|
*b = -S * D * ((T)MATH_DOUBLE_PIOVER2);
|
|
|
*c = S * D * atan2(psign * A[A2][A1], A[A2][A2]);
|
|
|
} else if (pm > T(1) - singularityRadius) { // North pole singularity
|
|
|
*a = T(0);
|
|
|
*b = S * D * ((T)MATH_DOUBLE_PIOVER2);
|
|
|
*c = S * D * atan2(psign * A[A2][A1], A[A2][A2]);
|
|
|
} else { // Normal case (nonsingular)
|
|
|
*a = S * D * atan2(-psign * A[A2][A3], A[A3][A3]);
|
|
|
*b = S * D * asin(pm);
|
|
|
*c = S * D * atan2(-psign * A[A1][A2], A[A1][A1]);
|
|
|
}
|
|
|
}
|
|
|
|
|
|
/* Matrix to Euler Angles conversion
|
|
|
* a,b,c, are the YawPitchRoll angles to be returned
|
|
|
* rotation a around axis A1
|
|
|
* is followed by rotation b around axis A2
|
|
|
* is followed by rotation c around axis A1
|
|
|
* rotations are CCW or CW (D) in LH or RH coordinate system (S)
|
|
|
*/
|
|
|
template<Axis A1, Axis A2, RotateDirection D, HandedSystem S> void ToEulerAnglesABA(T *a, T *b, T *c) const
|
|
|
{
|
|
|
IMAGE_MATH_STATIC_ASSERT(A1 != A2, "A1 != A2");
|
|
|
|
|
|
/* Determine the axis that was not supplied */
|
|
|
int m = 3 - A1 - A2;
|
|
|
|
|
|
T psign = T(-1);
|
|
|
if ((A1 + 1) % 3 == A2) /* average value for 3: A1<41><31>A2<41><32>A3 */
|
|
|
psign = T(1);
|
|
|
|
|
|
T c2 = A[A1][A1];
|
|
|
T singularityRadius = Math<T>::SingularityRadius();
|
|
|
if (c2 < T(-1) + singularityRadius) { // South pole singularity
|
|
|
*a = T(0);
|
|
|
*b = S * D * ((T)MATH_DOUBLE_PI);
|
|
|
*c = S * D * atan2(-psign * A[A2][m], A[A2][A2]);
|
|
|
} else if (c2 > T(1) - singularityRadius) { // North pole singularity
|
|
|
*a = T(0);
|
|
|
*b = T(0);
|
|
|
*c = S * D * atan2(-psign * A[A2][m], A[A2][A2]);
|
|
|
} else { // Normal case (nonsingular)
|
|
|
*a = S * D * atan2(A[A2][A1], -psign * A[m][A1]);
|
|
|
*b = S * D * acos(c2);
|
|
|
*c = S * D * atan2(A[A1][A2], psign * A[A1][m]);
|
|
|
}
|
|
|
}
|
|
|
|
|
|
/* Creates a matrix that converts the vertices from one coordinate system
|
|
|
* to another.
|
|
|
*/
|
|
|
static Matrix4 AxisConversion(const WorldAxes &to, const WorldAxes &from)
|
|
|
{
|
|
|
/* Holds axis values from the 'to' structure */
|
|
|
int toArray[3] = { to.axisX, to.axisY, to.axisZ};
|
|
|
|
|
|
/* The inverse of the toArray */
|
|
|
int inv[4];
|
|
|
inv[0] = inv[abs(to.axisX)] = 0;
|
|
|
inv[abs(to.axisY)] = 1;
|
|
|
inv[abs(to.axisZ)] = 2;
|
|
|
|
|
|
Matrix4 m(0, 0, 0, 0, 0, 0, 0, 0, 0);
|
|
|
|
|
|
/* Only three values in the matrix need to be changed to 1 or -1. */
|
|
|
m.A[inv[abs(from.axisX)]][0] = T(from.axisX / toArray[inv[abs(from.axisX)]]);
|
|
|
m.A[inv[abs(from.axisY)]][1] = T(from.axisY / toArray[inv[abs(from.axisY)]]);
|
|
|
m.A[inv[abs(from.axisZ)]][2] = T(from.axisZ / toArray[inv[abs(from.axisZ)]]);
|
|
|
return m;
|
|
|
}
|
|
|
|
|
|
/* Creates a matrix for translation by vector */
|
|
|
static Matrix4 Translation(T x, T y, T z = T(0))
|
|
|
{
|
|
|
Matrix4 t;
|
|
|
t.A[0][3] = x;
|
|
|
t.A[1][3] = y;
|
|
|
t.A[2][3] = z;
|
|
|
return t;
|
|
|
}
|
|
|
|
|
|
/* Creates a matrix for scaling by vector */
|
|
|
static Matrix4 Scaling(T x, T y, T z)
|
|
|
{
|
|
|
Matrix4 t;
|
|
|
t.A[0][0] = x;
|
|
|
t.A[1][1] = y;
|
|
|
t.A[2][2] = z;
|
|
|
return t;
|
|
|
}
|
|
|
|
|
|
/* Creates a matrix for scaling by constant */
|
|
|
static Matrix4 Scaling(T s)
|
|
|
{
|
|
|
Matrix4 t;
|
|
|
t.A[0][0] = s;
|
|
|
t.A[1][1] = s;
|
|
|
t.A[2][2] = s;
|
|
|
return t;
|
|
|
}
|
|
|
|
|
|
/* Simple L1 distance in R^12 */
|
|
|
T Distance(const Matrix4 &m2) const
|
|
|
{
|
|
|
T d = fabs(A[0][0] - m2.A[0][0]) + fabs(A[0][1] - m2.A[0][1]);
|
|
|
d += fabs(A[0][2] - m2.A[0][2]) + fabs(A[0][3] - m2.A[0][3]);
|
|
|
d += fabs(A[1][0] - m2.A[1][0]) + fabs(A[1][1] - m2.A[1][1]);
|
|
|
d += fabs(A[1][2] - m2.A[1][2]) + fabs(A[1][3] - m2.A[1][3]);
|
|
|
d += fabs(A[2][0] - m2.A[2][0]) + fabs(A[2][1] - m2.A[2][1]);
|
|
|
d += fabs(A[2][2] - m2.A[2][2]) + fabs(A[2][3] - m2.A[2][3]);
|
|
|
d += fabs(A[3][0] - m2.A[3][0]) + fabs(A[3][1] - m2.A[3][1]);
|
|
|
d += fabs(A[3][2] - m2.A[3][2]) + fabs(A[3][3] - m2.A[3][3]);
|
|
|
return d;
|
|
|
}
|
|
|
|
|
|
/* Creates a rotation matrix rotating around the X axis by 'angle' radians.
|
|
|
* Just for quick testing. Not for final API. Need to remove case.
|
|
|
*/
|
|
|
static Matrix4 RotationAxis(Axis A, T angle, RotateDirection d, HandedSystem s)
|
|
|
{
|
|
|
T sina = s * d * sin(angle);
|
|
|
T cosa = cos(angle);
|
|
|
|
|
|
switch (A) {
|
|
|
case AXIS_X:
|
|
|
return Matrix4(1, 0, 0, 0, cosa, -sina, 0, sina, cosa);
|
|
|
case AXIS_Y:
|
|
|
return Matrix4(cosa, 0, sina, 0, 1, 0, -sina, 0, cosa);
|
|
|
case AXIS_Z:
|
|
|
return Matrix4(cosa, -sina, 0, sina, cosa, 0, 0, 0, 1);
|
|
|
default:
|
|
|
return Matrix4();
|
|
|
}
|
|
|
}
|
|
|
|
|
|
/* Creates a rotation matrix rotating around the X axis by 'angle' radians.
|
|
|
* Rotation direction is depends on the coordinate system:
|
|
|
* RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
|
|
|
* while looking in the negative axis direction. This is the
|
|
|
* same as looking down from positive axis values towards origin.
|
|
|
* LHS: Positive angle values rotate clock-wise (CW), while looking in the
|
|
|
* negative axis direction.
|
|
|
*/
|
|
|
static Matrix4 RotationX(T angle)
|
|
|
{
|
|
|
T sina = sin(angle);
|
|
|
T cosa = cos(angle);
|
|
|
return Matrix4(1, 0, 0, 0, cosa, -sina, 0, sina, cosa);
|
|
|
}
|
|
|
|
|
|
/* Creates a rotation matrix rotating around the Y axis by 'angle' radians.
|
|
|
* Rotation direction is depends on the coordinate system:
|
|
|
* RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
|
|
|
* while looking in the negative axis direction. This is the
|
|
|
* same as looking down from positive axis values towards origin.
|
|
|
* LHS: Positive angle values rotate clock-wise (CW), while looking in the
|
|
|
* negative axis direction.
|
|
|
*/
|
|
|
static Matrix4 RotationY(T angle)
|
|
|
{
|
|
|
T sina = sin(angle);
|
|
|
T cosa = cos(angle);
|
|
|
return Matrix4(cosa, 0, sina, 0, 1, 0, -sina, 0, cosa);
|
|
|
}
|
|
|
|
|
|
/* Creates a rotation matrix rotating around the Z axis by 'angle' radians.
|
|
|
* Rotation direction is depends on the coordinate system:
|
|
|
* RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
|
|
|
* while looking in the negative axis direction. This is the
|
|
|
* same as looking down from positive axis values towards origin.
|
|
|
* LHS: Positive angle values rotate clock-wise (CW), while looking in the
|
|
|
* negative axis direction.
|
|
|
*/
|
|
|
static Matrix4 RotationZ(T angle)
|
|
|
{
|
|
|
T sina = sin(angle);
|
|
|
T cosa = cos(angle);
|
|
|
return Matrix4(cosa, -sina, 0, sina, cosa, 0, 0, 0, 1);
|
|
|
}
|
|
|
|
|
|
/* PerspectiveRH creates a right-handed perspective projection matrix that can be
|
|
|
* used with the Oculus sample renderer.
|
|
|
* yfov - Specifies vertical field of view in radians.
|
|
|
* aspect - Screen aspect ration, which is usually width/height for square pixels.
|
|
|
* Note that xfov = yfov * aspect.
|
|
|
* znear - Absolute value of near Z clipping clipping range.
|
|
|
* zfar - Absolute value of far Z clipping clipping range (larger then near).
|
|
|
* Even though RHS usually looks in the direction of negative Z, positive values
|
|
|
* are expected for znear and zfar.
|
|
|
*/
|
|
|
static Matrix4 PerspectiveRH(T yfov, T aspect, T znear, T zfar)
|
|
|
{
|
|
|
Matrix4 m;
|
|
|
T tanHalfFov = tan(yfov * T(0.5));
|
|
|
|
|
|
m.A[0][0] = T(1) / (aspect * tanHalfFov);
|
|
|
m.A[1][1] = T(1) / tanHalfFov;
|
|
|
m.A[2][2] = zfar / (znear - zfar);
|
|
|
m.A[3][2] = T(-1);
|
|
|
m.A[2][3] = (zfar * znear) / (znear - zfar);
|
|
|
m.A[3][3] = T(0);
|
|
|
|
|
|
/* Note: Post-projection matrix result assumes Left-Handed coordinate system,
|
|
|
* with Y up, X right and Z forward. This supports positive z-buffer values.
|
|
|
* This is the case even for RHS coordinate input.
|
|
|
*/
|
|
|
return m;
|
|
|
}
|
|
|
|
|
|
/* PerspectiveLH creates a left-handed perspective projection matrix that can be
|
|
|
* used with the Oculus sample renderer.
|
|
|
* yfov - Specifies vertical field of view in radians.
|
|
|
* aspect - Screen aspect ration, which is usually width/height for square pixels.
|
|
|
* Note that xfov = yfov * aspect.
|
|
|
* znear - Absolute value of near Z clipping clipping range.
|
|
|
* zfar - Absolute value of far Z clipping clipping range (larger then near).
|
|
|
*/
|
|
|
static Matrix4 PerspectiveLH(T yfov, T aspect, T znear, T zfar)
|
|
|
{
|
|
|
Matrix4 m;
|
|
|
T tanHalfFov = tan(yfov * T(0.5));
|
|
|
|
|
|
m.A[0][0] = T(1) / (aspect * tanHalfFov);
|
|
|
m.A[1][1] = T(1) / tanHalfFov;
|
|
|
m.A[2][2] = zfar / (zfar - znear);
|
|
|
m.A[3][2] = T(-1);
|
|
|
m.A[2][3] = (zfar * znear) / (znear - zfar);
|
|
|
m.A[3][3] = T(0);
|
|
|
|
|
|
/* Note: Post-projection matrix result assumes Left-Handed coordinate system,
|
|
|
* with Y up, X right and Z forward. This supports positive z-buffer values.
|
|
|
* This is the case even for RHS coordinate input.
|
|
|
*/
|
|
|
return m;
|
|
|
}
|
|
|
|
|
|
static Matrix4 PerspectiveLR(T yfov, T aspect, T znear, T zfar)
|
|
|
{
|
|
|
Matrix4 m;
|
|
|
T tanHalfFov = tan(yfov * T(0.5));
|
|
|
|
|
|
m.A[0][0] = T(1) / tanHalfFov;
|
|
|
m.A[1][1] = T(1) * aspect / tanHalfFov;
|
|
|
m.A[2][2] = -(zfar + znear) / (zfar - znear);
|
|
|
m.A[3][2] = T(-1);
|
|
|
m.A[2][3] = (zfar * znear) / (znear - zfar);
|
|
|
m.A[3][3] = T(0);
|
|
|
|
|
|
/* Note: Post-projection matrix result assumes Left-Handed coordinate system,
|
|
|
* with Y up, X right and Z forward. This supports positive z-buffer values.
|
|
|
* This is the case even for RHS coordinate input.
|
|
|
*/
|
|
|
|
|
|
return m;
|
|
|
}
|
|
|
|
|
|
static Matrix4 Ortho2D(T w, T h)
|
|
|
{
|
|
|
Matrix4 m;
|
|
|
m.A[0][0] = T(2.0) / w;
|
|
|
m.A[1][1] = T(-2.0) / h;
|
|
|
m.A[0][3] = T(-1.0);
|
|
|
m.A[1][3] = T(1.0);
|
|
|
m.A[2][2] = T(0);
|
|
|
return m;
|
|
|
}
|
|
|
};
|
|
|
|
|
|
typedef Matrix4<float> Matrix4f;
|
|
|
typedef Matrix4<double> Matrix4d;
|
|
|
}
|
|
|
|
|
|
|
|
|
#endif
|
