/*
* vec_mat.h - vector and matrix defination & calculation
*
* Copyright (c) 2017 Intel Corporation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Author: Zong Wei <wei.zong@intel.com>
*/
#ifndef XCAM_VECTOR_MATRIX_H
#define XCAM_VECTOR_MATRIX_H
#include <xcam_std.h>
#include <cmath>
namespace XCam {
#ifndef PI
#define PI 3.14159265358979323846
#endif
#ifndef FLT_EPSILON
#define FLT_EPSILON 1.19209290e-07F // float
#endif
#ifndef DBL_EPSILON
#define DBL_EPSILON 2.2204460492503131e-16 // double
#endif
#ifndef DEGREE_2_RADIANS
#define DEGREE_2_RADIANS(x) (((x) * PI) / 180.0)
#endif
#ifndef RADIANS_2_DEGREE
#define RADIANS_2_DEGREE(x) (((x) * 180.0) / PI)
#endif
#define XCAM_VECT2_OPERATOR_VECT2(op) \
Vector2<T> operator op (const Vector2<T>& b) const { \
return Vector2<T>(x op b.x, y op b.y); \
} \
Vector2<T> &operator op##= (const Vector2<T>& b) { \
x op##= b.x; y op##= b.y; return *this; \
}
#define XCAM_VECT2_OPERATOR_SCALER(op) \
Vector2<T> operator op (const T& b) const { \
return Vector2<T>(x op b, y op b); \
} \
Vector2<T> &operator op##= (const T& b) { \
x op##= b; y op##= b; return *this; \
}
template<class T>
class Vector2
{
public:
T x;
T y;
Vector2 () : x(0), y(0) {};
Vector2 (T _x, T _y) : x(_x), y(_y) {};
template <typename New>
Vector2<New> convert_to () const {
Vector2<New> ret((New)(this->x), (New)(this->y));
return ret;
}
Vector2<T>& operator = (const Vector2<T>& rhs)
{
x = rhs.x;
y = rhs.y;
return *this;
}
template <typename Other>
Vector2<T>& operator = (const Vector2<Other>& rhs)
{
x = rhs.x;
y = rhs.y;
return *this;
}
Vector2<T> operator - () const {
return Vector2<T>(-x, -y);
}
XCAM_VECT2_OPERATOR_VECT2 (+)
XCAM_VECT2_OPERATOR_VECT2 (-)
XCAM_VECT2_OPERATOR_VECT2 (*)
XCAM_VECT2_OPERATOR_VECT2 ( / )
XCAM_VECT2_OPERATOR_SCALER (+)
XCAM_VECT2_OPERATOR_SCALER (-)
XCAM_VECT2_OPERATOR_SCALER (*)
XCAM_VECT2_OPERATOR_SCALER ( / )
bool operator == (const Vector2<T>& rhs) const {
return (x == rhs.x) && (y == rhs.y);
}
void reset () {
this->x = (T) 0;
this->y = (T) 0;
}
void set (T _x, T _y) {
this->x = _x;
this->y = _y;
}
T magnitude () const {
return (T) sqrtf(x * x + y * y);
}
float distance (const Vector2<T>& vec) const {
return sqrtf((vec.x - x) * (vec.x - x) + (vec.y - y) * (vec.y - y));
}
T dot (const Vector2<T>& vec) const {
return (x * vec.x + y * vec.y);
}
inline Vector2<T> lerp (T weight, const Vector2<T>& vec) const {
return (*this) + (vec - (*this)) * weight;
}
};
template<class T, uint32_t N>
class VectorN
{
public:
VectorN ();
VectorN (T x);
VectorN (T x, T y);
VectorN (T x, T y, T z);
VectorN (T x, T y, T z, T w);
VectorN (VectorN<T, 3> vec3, T w);
inline VectorN<T, N>& operator = (const VectorN<T, N>& rhs);
inline VectorN<T, N> operator - () const;
inline bool operator == (const VectorN<T, N>& rhs) const;
inline T& operator [] (uint32_t index) {
XCAM_ASSERT(index >= 0 && index < N);
return data[index];
}
inline const T& operator [] (uint32_t index) const {
XCAM_ASSERT(index >= 0 && index < N);
return data[index];
}
inline VectorN<T, N> operator + (const T rhs) const;
inline VectorN<T, N> operator - (const T rhs) const;
inline VectorN<T, N> operator * (const T rhs) const;
inline VectorN<T, N> operator / (const T rhs) const;
inline VectorN<T, N> operator += (const T rhs);
inline VectorN<T, N> operator -= (const T rhs);
inline VectorN<T, N> operator *= (const T rhs);
inline VectorN<T, N> operator /= (const T rhs);
inline VectorN<T, N> operator + (const VectorN<T, N>& rhs) const;
inline VectorN<T, N> operator - (const VectorN<T, N>& rhs) const;
inline VectorN<T, N> operator * (const VectorN<T, N>& rhs) const;
inline VectorN<T, N> operator / (const VectorN<T, N>& rhs) const;
inline VectorN<T, N> operator += (const VectorN<T, N>& rhs);
inline VectorN<T, N> operator -= (const VectorN<T, N>& rhs);
inline VectorN<T, N> operator *= (const VectorN<T, N>& rhs);
inline VectorN<T, N> operator /= (const VectorN<T, N>& rhs);
template <typename NEW> inline
VectorN<NEW, N> convert_to () const;
inline void zeros ();
inline void set (T x, T y);
inline void set (T x, T y, T z);
inline void set (T x, T y, T z, T w);
inline T magnitude () const;
inline float distance (const VectorN<T, N>& vec) const;
inline T dot (const VectorN<T, N>& vec) const;
inline VectorN<T, N> lerp (T weight, const VectorN<T, N>& vec) const;
private:
T data[N];
};
template<class T, uint32_t N> inline
VectorN<T, N>::VectorN ()
{
for (uint32_t i = 0; i < N; i++) {
data[i] = 0;
}
}
template<class T, uint32_t N> inline
VectorN<T, N>::VectorN (T x) {
data[0] = x;
}
template<class T, uint32_t N> inline
VectorN<T, N>::VectorN (T x, T y) {
if (N >= 2) {
data[0] = x;
data[1] = y;
}
}
template<class T, uint32_t N> inline
VectorN<T, N>::VectorN (T x, T y, T z) {
if (N >= 3) {
data[0] = x;
data[1] = y;
data[2] = z;
}
}
template<class T, uint32_t N> inline
VectorN<T, N>::VectorN (T x, T y, T z, T w) {
if (N >= 4) {
data[0] = x;
data[1] = y;
data[2] = z;
data[3] = w;
}
}
template<class T, uint32_t N> inline
VectorN<T, N>::VectorN (VectorN<T, 3> vec3, T w) {
if (N >= 4) {
data[0] = vec3.data[0];
data[1] = vec3.data[1];
data[2] = vec3.data[2];
data[3] = w;
}
}
template<class T, uint32_t N> inline
VectorN<T, N>& VectorN<T, N>::operator = (const VectorN<T, N>& rhs) {
for (uint32_t i = 0; i < N; i++) {
data[i] = rhs.data[i];
}
return *this;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator - () const {
for (uint32_t i = 0; i < N; i++) {
data[i] = -data[i];
}
return *this;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator + (const T rhs) const {
VectorN<T, N> result;
for (uint32_t i = 0; i < N; i++) {
result.data[i] = data[i] + rhs;
}
return result;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator - (const T rhs) const {
VectorN<T, N> result;
for (uint32_t i = 0; i < N; i++) {
result.data[i] = data[i] - rhs;
}
return result;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator * (const T rhs) const {
VectorN<T, N> result;
for (uint32_t i = 0; i < N; i++) {
result.data[i] = data[i] * rhs;
}
return result;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator / (const T rhs) const {
VectorN<T, N> result;
for (uint32_t i = 0; i < N; i++) {
result.data[i] = data[i] / rhs;
}
return result;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator += (const T rhs) {
for (uint32_t i = 0; i < N; i++) {
data[i] += rhs;
}
return *this;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator -= (const T rhs) {
for (uint32_t i = 0; i < N; i++) {
data[i] -= rhs;
}
return *this;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator *= (const T rhs) {
for (uint32_t i = 0; i < N; i++) {
data[i] *= rhs;
}
return *this;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator /= (const T rhs) {
for (uint32_t i = 0; i < N; i++) {
data[i] /= rhs;
}
return *this;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator + (const VectorN<T, N>& rhs) const {
VectorN<T, N> result;
for (uint32_t i = 0; i < N; i++) {
result.data[i] = data[i] + rhs.data[i];
}
return result;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator - (const VectorN<T, N>& rhs) const {
VectorN<T, N> result;
for (uint32_t i = 0; i < N; i++) {
result.data[i] = data[i] - rhs.data[i];
}
return result;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator * (const VectorN<T, N>& rhs) const {
VectorN<T, N> result;
for (uint32_t i = 0; i < N; i++) {
result.data[i] = data[i] * rhs.data[i];
}
return result;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator / (const VectorN<T, N>& rhs) const {
VectorN<T, N> result;
for (uint32_t i = 0; i < N; i++) {
result.data[i] = data[i] / rhs.data[i];
}
return result;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator += (const VectorN<T, N>& rhs) {
for (uint32_t i = 0; i < N; i++) {
data[i] += rhs.data[i];
}
return *this;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator -= (const VectorN<T, N>& rhs) {
for (uint32_t i = 0; i < N; i++) {
data[i] -= rhs.data[i];
}
return *this;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator *= (const VectorN<T, N>& rhs) {
for (uint32_t i = 0; i < N; i++) {
data[i] *= rhs.data[i];
}
return *this;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::operator /= (const VectorN<T, N>& rhs) {
for (uint32_t i = 0; i < N; i++) {
data[i] /= rhs.data[i];
}
return *this;
}
template<class T, uint32_t N> inline
bool VectorN<T, N>::operator == (const VectorN<T, N>& rhs) const {
for (uint32_t i = 0; i < N; i++) {
if (data[i] != rhs[i]) {
return false;
}
}
return true;
}
template <class T, uint32_t N>
template <typename NEW>
VectorN<NEW, N> VectorN<T, N>::convert_to () const {
VectorN<NEW, N> result;
for (uint32_t i = 0; i < N; i++) {
result[i] = (NEW)(this->data[i]);
}
return result;
}
template <class T, uint32_t N> inline
void VectorN<T, N>::zeros () {
for (uint32_t i = 0; i < N; i++) {
data[i] = (T)(0);
}
}
template<class T, uint32_t N> inline
void VectorN<T, N>::set (T x, T y) {
if (N >= 2) {
data[0] = x;
data[1] = y;
}
}
template<class T, uint32_t N> inline
void VectorN<T, N>::set (T x, T y, T z) {
if (N >= 3) {
data[0] = x;
data[1] = y;
data[2] = z;
}
}
template<class T, uint32_t N> inline
void VectorN<T, N>::set (T x, T y, T z, T w) {
if (N >= 4) {
data[0] - x;
data[1] = y;
data[2] = z;
data[3] = w;
}
}
template<class T, uint32_t N> inline
T VectorN<T, N>::magnitude () const {
T result = 0;
for (uint32_t i = 0; i < N; i++) {
result += (data[i] * data[i]);
}
return (T) sqrtf(result);
}
template<class T, uint32_t N> inline
float VectorN<T, N>::distance (const VectorN<T, N>& vec) const {
T result = 0;
for (uint32_t i = 0; i < N; i++) {
result += (vec.data[i] - data[i]) * (vec.data[i] - data[i]);
}
return sqrtf(result);
}
template<class T, uint32_t N> inline
T VectorN<T, N>::dot (const VectorN<T, N>& vec) const {
T result = 0;
for (uint32_t i = 0; i < N; i++) {
result += (vec.data[i] * data[i]);
}
return result;
}
template<class T, uint32_t N> inline
VectorN<T, N> VectorN<T, N>::lerp (T weight, const VectorN<T, N>& vec) const {
return (*this) + (vec - (*this)) * weight;
}
// NxN matrix in row major order
template<class T, uint32_t N>
class MatrixN
{
public:
MatrixN ();
MatrixN (VectorN<T, 2> a, VectorN<T, 2> b);
MatrixN (VectorN<T, 3> a, VectorN<T, 3> b, VectorN<T, 3> c);
MatrixN (VectorN<T, 4> a, VectorN<T, 4> b, VectorN<T, 4> c, VectorN<T, 4> d);
inline void zeros ();
inline void eye ();
inline T& at (uint32_t row, uint32_t col) {
XCAM_ASSERT(row >= 0 && row < N);
XCAM_ASSERT(col >= 0 && col < N);
return data[row * N + col];
};
inline const T& at (uint32_t row, uint32_t col) const {
XCAM_ASSERT(row >= 0 && row < N);
XCAM_ASSERT(col >= 0 && col < N);
return data[row * N + col];
};
inline T& operator () (uint32_t row, uint32_t col) {
return at (row, col);
};
inline const T& operator () (uint32_t row, uint32_t col) const {
return at (row, col);
};
inline MatrixN<T, N>& operator = (const MatrixN<T, N>& rhs);
inline MatrixN<T, N> operator - () const;
inline MatrixN<T, N> operator + (const MatrixN<T, N>& rhs) const;
inline MatrixN<T, N> operator - (const MatrixN<T, N>& rhs) const;
inline MatrixN<T, N> operator * (const T a) const;
inline MatrixN<T, N> operator / (const T a) const;
inline VectorN<T, N> operator * (const VectorN<T, N>& rhs) const;
inline MatrixN<T, N> operator * (const MatrixN<T, N>& rhs) const;
inline MatrixN<T, N> transpose ();
inline MatrixN<T, N> inverse ();
inline T trace ();
private:
inline MatrixN<T, 2> inverse (const MatrixN<T, 2>& mat);
inline MatrixN<T, 3> inverse (const MatrixN<T, 3>& mat);
inline MatrixN<T, 4> inverse (const MatrixN<T, 4>& mat);
private:
T data[N * N];
};
// NxN matrix in row major order
template<class T, uint32_t N>
MatrixN<T, N>::MatrixN () {
eye ();
}
template<class T, uint32_t N>
MatrixN<T, N>::MatrixN (VectorN<T, 2> a, VectorN<T, 2> b) {
if (N == 2) {
data[0] = a[0];
data[1] = a[1];
data[2] = b[0];
data[3] = b[1];
} else {
eye ();
}
}
template<class T, uint32_t N>
MatrixN<T, N>::MatrixN (VectorN<T, 3> a, VectorN<T, 3> b, VectorN<T, 3> c) {
if (N == 3) {
data[0] = a[0];
data[1] = a[1];
data[2] = a[2];
data[3] = b[0];
data[4] = b[1];
data[5] = b[2];
data[6] = c[0];
data[7] = c[1];
data[8] = c[2];
} else {
eye ();
}
}
template<class T, uint32_t N>
MatrixN<T, N>::MatrixN (VectorN<T, 4> a, VectorN<T, 4> b, VectorN<T, 4> c, VectorN<T, 4> d) {
if (N == 4) {
data[0] = a[0];
data[1] = a[1];
data[2] = a[2];
data[3] = a[3];
data[4] = b[0];
data[5] = b[1];
data[6] = b[2];
data[7] = b[3];
data[8] = c[0];
data[9] = c[1];
data[10] = c[2];
data[11] = c[3];
data[12] = d[0];
data[13] = d[1];
data[14] = d[2];
data[15] = d[3];
} else {
eye ();
}
}
template<class T, uint32_t N> inline
void MatrixN<T, N>::zeros () {
for (uint32_t i = 0; i < N * N; i++) {
data[i] = 0;
}
}
template<class T, uint32_t N> inline
void MatrixN<T, N>::eye () {
zeros ();
for (uint32_t i = 0; i < N; i++) {
data[i * N + i] = 1;
}
}
template<class T, uint32_t N> inline
MatrixN<T, N>& MatrixN<T, N>::operator = (const MatrixN<T, N>& rhs) {
for (uint32_t i = 0; i < N * N; i++) {
data[i] = rhs.data[i];
}
return *this;
}
template<class T, uint32_t N> inline
MatrixN<T, N> MatrixN<T, N>::operator - () const {
MatrixN<T, N> result;
for (uint32_t i = 0; i < N * N; i++) {
result.data[i] = -data[i];
}
return result;
}
template<class T, uint32_t N> inline
MatrixN<T, N> MatrixN<T, N>::operator + (const MatrixN<T, N>& rhs) const {
MatrixN<T, N> result;
for (uint32_t i = 0; i < N * N; i++) {
result.data[i] = data[i] + rhs.data[i];
}
return result;
}
template<class T, uint32_t N> inline
MatrixN<T, N> MatrixN<T, N>::operator - (const MatrixN<T, N>& rhs) const {
MatrixN<T, N> result;
for (uint32_t i = 0; i < N * N; i++) {
result.data[i] = data[i] - rhs.data[i];
}
return result;
}
template<class T, uint32_t N> inline
MatrixN<T, N> MatrixN<T, N>::operator * (const T a) const {
MatrixN<T, N> result;
for (uint32_t i = 0; i < N * N; i++) {
result.data[i] = data[i] * a;
}
return result;
}
template<class T, uint32_t N> inline
MatrixN<T, N> MatrixN<T, N>::operator / (const T a) const {
MatrixN<T, N> result;
for (uint32_t i = 0; i < N * N; i++) {
result.data[i] = data[i] / a;
}
return result;
}
template<class T, uint32_t N> inline
MatrixN<T, N> MatrixN<T, N>::operator * (const MatrixN<T, N>& rhs) const {
MatrixN<T, N> result;
result.zeros ();
for (uint32_t i = 0; i < N; i++) {
for (uint32_t j = 0; j < N; j++) {
T element = 0;
for (uint32_t k = 0; k < N; k++) {
element += at(i, k) * rhs(k, j);
}
result(i, j) = element;
}
}
return result;
}
template<class T, uint32_t N> inline
VectorN<T, N> MatrixN<T, N>::operator * (const VectorN<T, N>& rhs) const {
VectorN<T, N> result;
for (uint32_t i = 0; i < N; i++) { // row
for (uint32_t j = 0; j < N; j++) { // col
result.data[i] = data[i * N + j] * rhs.data[j];
}
}
return result;
}
template<class T, uint32_t N> inline
MatrixN<T, N> MatrixN<T, N>::transpose () {
MatrixN<T, N> result;
for (uint32_t i = 0; i < N; i++) {
for (uint32_t j = 0; j <= N; j++) {
result.data[i * N + j] = data[j * N + i];
}
}
return result;
}
// if the matrix is non-invertible, return identity matrix
template<class T, uint32_t N> inline
MatrixN<T, N> MatrixN<T, N>::inverse () {
MatrixN<T, N> result;
result = inverse (*this);
return result;
}
template<class T, uint32_t N> inline
T MatrixN<T, N>::trace () {
T t = 0;
for ( uint32_t i = 0; i < N; i++ ) {
t += data(i, i);
}
return t;
}
template<class T, uint32_t N> inline
MatrixN<T, 2> MatrixN<T, N>::inverse (const MatrixN<T, 2>& mat)
{
MatrixN<T, 2> result;
T det = mat(0, 0) * mat(1, 1) - mat(0, 1) * mat(1, 0);
if (det == (T)0) {
return result;
}
result(0, 0) = mat(1, 1);
result(0, 1) = -mat(0, 1);
result(1, 0) = -mat(1, 0);
result(1, 1) = mat(0, 0);
return result * (1.0f / det);
}
template<class T, uint32_t N> inline
MatrixN<T, 3> MatrixN<T, N>::inverse (const MatrixN<T, 3>& mat)
{
MatrixN<T, 3> result;
T det = mat(0, 0) * mat(1, 1) * mat(2, 2) +
mat(1, 0) * mat(2, 1) * mat(0, 2) +
mat(2, 0) * mat(0, 1) * mat(1, 2) -
mat(0, 0) * mat(2, 1) * mat(1, 2) -
mat(1, 0) * mat(0, 1) * mat(2, 2) -
mat(2, 0) * mat(1, 1) * mat(0, 2);
if (det == (T)0) {
return result;
}
result(0, 0) = mat(1, 1) * mat(2, 2) - mat(1, 2) * mat(2, 1);
result(1, 0) = mat(1, 2) * mat(2, 0) - mat(1, 0) * mat(2, 2);
result(2, 0) = mat(1, 0) * mat(2, 1) - mat(1, 1) * mat(2, 0);
result(0, 1) = mat(0, 2) * mat(2, 1) - mat(0, 1) * mat(2, 2);
result(1, 1) = mat(0, 0) * mat(2, 2) - mat(0, 2) * mat(2, 0);
result(2, 1) = mat(0, 1) * mat(2, 0) - mat(0, 0) * mat(2, 1);
result(0, 2) = mat(0, 1) * mat(1, 2) - mat(0, 2) * mat(1, 1);
result(1, 2) = mat(0, 2) * mat(1, 0) - mat(0, 0) * mat(1, 2);
result(2, 2) = mat(0, 0) * mat(1, 1) - mat(0, 1) * mat(1, 0);
return result * (1.0f / det);
}
template<class T, uint32_t N> inline
MatrixN<T, 4> MatrixN<T, N>::inverse (const MatrixN<T, 4>& mat)
{
MatrixN<T, 4> result;
T det = mat(0, 3) * mat(1, 2) * mat(2, 1) * mat(3, 1) -
mat(0, 2) * mat(1, 3) * mat(2, 1) * mat(3, 1) -
mat(0, 3) * mat(1, 1) * mat(2, 2) * mat(3, 1) +
mat(0, 1) * mat(1, 3) * mat(2, 2) * mat(3, 1) +
mat(0, 2) * mat(1, 1) * mat(2, 3) * mat(3, 1) -
mat(0, 1) * mat(1, 2) * mat(2, 3) * mat(3, 1) -
mat(0, 3) * mat(1, 2) * mat(2, 0) * mat(3, 1) +
mat(0, 2) * mat(1, 3) * mat(2, 0) * mat(3, 1) +
mat(0, 3) * mat(1, 0) * mat(2, 2) * mat(3, 1) -
mat(0, 0) * mat(1, 3) * mat(2, 2) * mat(3, 1) -
mat(0, 2) * mat(1, 0) * mat(2, 3) * mat(3, 1) +
mat(0, 0) * mat(1, 2) * mat(2, 3) * mat(3, 1) +
mat(0, 3) * mat(1, 1) * mat(2, 0) * mat(3, 2) -
mat(0, 1) * mat(1, 3) * mat(2, 0) * mat(3, 2) -
mat(0, 3) * mat(1, 0) * mat(2, 1) * mat(3, 2) +
mat(0, 0) * mat(1, 3) * mat(2, 1) * mat(3, 2) +
mat(0, 1) * mat(1, 0) * mat(2, 3) * mat(3, 2) -
mat(0, 0) * mat(1, 1) * mat(2, 3) * mat(3, 2) -
mat(0, 2) * mat(1, 1) * mat(2, 0) * mat(3, 3) +
mat(0, 1) * mat(1, 2) * mat(2, 0) * mat(3, 3) +
mat(0, 2) * mat(1, 0) * mat(2, 1) * mat(3, 3) -
mat(0, 0) * mat(1, 2) * mat(2, 1) * mat(3, 3) -
mat(0, 1) * mat(1, 0) * mat(2, 2) * mat(3, 3) +
mat(0, 0) * mat(1, 1) * mat(2, 2) * mat(3, 3);
if (det == (T)0) {
return result;
}
result(0, 0) = mat(1, 2) * mat(2, 3) * mat(3, 1) -
mat(1, 3) * mat(2, 2) * mat(3, 1) +
mat(1, 3) * mat(2, 1) * mat(3, 2) -
mat(1, 1) * mat(2, 3) * mat(3, 2) -
mat(1, 2) * mat(2, 1) * mat(3, 3) +
mat(1, 1) * mat(2, 2) * mat(3, 3);
result(0, 1) = mat(0, 3) * mat(2, 2) * mat(3, 1) -
mat(0, 2) * mat(2, 3) * mat(3, 1) -
mat(0, 3) * mat(2, 1) * mat(3, 2) +
mat(0, 1) * mat(2, 3) * mat(3, 2) +
mat(0, 2) * mat(2, 1) * mat(3, 3) -
mat(0, 1) * mat(2, 2) * mat(3, 3);
result(0, 2) = mat(0, 2) * mat(1, 3) * mat(3, 1) -
mat(0, 3) * mat(1, 2) * mat(3, 1) +
mat(0, 3) * mat(1, 1) * mat(3, 2) -
mat(0, 1) * mat(1, 3) * mat(3, 2) -
mat(0, 2) * mat(1, 1) * mat(3, 3) +
mat(0, 1) * mat(1, 2) * mat(3, 3);
result(0, 3) = mat(0, 3) * mat(1, 2) * mat(2, 1) -
mat(0, 2) * mat(1, 3) * mat(2, 1) -
mat(0, 3) * mat(1, 1) * mat(2, 2) +
mat(0, 1) * mat(1, 3) * mat(2, 2) +
mat(0, 2) * mat(1, 1) * mat(2, 3) -
mat(0, 1) * mat(1, 2) * mat(2, 3);
result(1, 0) = mat(1, 3) * mat(2, 2) * mat(3, 0) -
mat(1, 2) * mat(2, 3) * mat(3, 0) -
mat(1, 3) * mat(2, 0) * mat(3, 2) +
mat(1, 0) * mat(2, 3) * mat(3, 2) +
mat(1, 2) * mat(2, 0) * mat(3, 3) -
mat(1, 0) * mat(2, 2) * mat(3, 3);
result(1, 1) = mat(0, 2) * mat(2, 3) * mat(3, 0) -
mat(0, 3) * mat(2, 2) * mat(3, 0) +
mat(0, 3) * mat(2, 0) * mat(3, 2) -
mat(0, 0) * mat(2, 3) * mat(3, 2) -
mat(0, 2) * mat(2, 0) * mat(3, 3) +
mat(0, 0) * mat(2, 2) * mat(3, 3);
result(1, 2) = mat(0, 3) * mat(1, 2) * mat(3, 0) -
mat(0, 2) * mat(1, 3) * mat(3, 0) -
mat(0, 3) * mat(1, 0) * mat(3, 2) +
mat(0, 0) * mat(1, 3) * mat(3, 2) +
mat(0, 2) * mat(1, 0) * mat(3, 3) -
mat(0, 0) * mat(1, 2) * mat(3, 3);
result(1, 3) = mat(0, 2) * mat(1, 3) * mat(2, 0) -
mat(0, 3) * mat(1, 2) * mat(2, 0) +
mat(0, 3) * mat(1, 0) * mat(2, 2) -
mat(0, 0) * mat(1, 3) * mat(2, 2) -
mat(0, 2) * mat(1, 0) * mat(2, 3) +
mat(0, 0) * mat(1, 2) * mat(2, 3);
result(2, 0) = mat(1, 1) * mat(2, 3) * mat(3, 0) -
mat(1, 3) * mat(2, 1) * mat(3, 0) +
mat(1, 3) * mat(2, 0) * mat(3, 1) -
mat(1, 0) * mat(2, 3) * mat(3, 1) -
mat(1, 1) * mat(2, 0) * mat(3, 3) +
mat(1, 0) * mat(2, 1) * mat(3, 3);
result(2, 1) = mat(0, 3) * mat(2, 1) * mat(3, 0) -
mat(0, 1) * mat(2, 3) * mat(3, 0) -
mat(0, 3) * mat(2, 0) * mat(3, 1) +
mat(0, 0) * mat(2, 3) * mat(3, 1) +
mat(0, 1) * mat(2, 0) * mat(3, 3) -
mat(0, 0) * mat(2, 1) * mat(3, 3);
result(2, 2) = mat(0, 1) * mat(1, 3) * mat(3, 0) -
mat(0, 3) * mat(1, 1) * mat(3, 0) +
mat(0, 3) * mat(1, 0) * mat(3, 1) -
mat(0, 0) * mat(1, 3) * mat(3, 1) -
mat(0, 1) * mat(1, 0) * mat(3, 3) +
mat(0, 0) * mat(1, 1) * mat(3, 3);
result(2, 3) = mat(0, 3) * mat(1, 1) * mat(2, 0) -
mat(0, 1) * mat(1, 3) * mat(2, 0) -
mat(0, 3) * mat(1, 0) * mat(2, 1) +
mat(0, 0) * mat(1, 3) * mat(2, 1) +
mat(0, 1) * mat(1, 0) * mat(2, 3) -
mat(0, 0) * mat(1, 1) * mat(2, 3);
result(3, 0) = mat(1, 2) * mat(2, 1) * mat(3, 0) -
mat(1, 1) * mat(2, 2) * mat(3, 0) -
mat(1, 2) * mat(2, 0) * mat(3, 1) +
mat(1, 0) * mat(2, 2) * mat(3, 1) +
mat(1, 1) * mat(2, 0) * mat(3, 2) -
mat(1, 0) * mat(2, 1) * mat(3, 2);
result(3, 1) = mat(1, 1) * mat(2, 2) * mat(3, 0) -
mat(1, 2) * mat(2, 1) * mat(3, 0) +
mat(1, 2) * mat(2, 0) * mat(3, 1) -
mat(1, 0) * mat(2, 2) * mat(3, 1) -
mat(1, 1) * mat(2, 0) * mat(3, 2) +
mat(1, 0) * mat(2, 1) * mat(3, 2);
result(3, 2) = mat(0, 2) * mat(1, 1) * mat(3, 0) -
mat(0, 1) * mat(1, 2) * mat(3, 0) -
mat(0, 2) * mat(1, 0) * mat(3, 1) +
mat(0, 0) * mat(1, 2) * mat(3, 1) +
mat(0, 1) * mat(1, 0) * mat(3, 2) -
mat(0, 0) * mat(1, 1) * mat(3, 2);
result(3, 3) = mat(0, 1) * mat(1, 2) * mat(2, 0) -
mat(0, 2) * mat(1, 1) * mat(2, 0) +
mat(0, 2) * mat(1, 0) * mat(2, 1) -
mat(0, 0) * mat(1, 2) * mat(2, 1) -
mat(0, 1) * mat(1, 0) * mat(2, 2) +
mat(0, 0) * mat(1, 1) * mat(2, 2);
return result * (1.0f / det);
}
typedef VectorN<double, 2> Vec2d;
typedef VectorN<double, 3> Vec3d;
typedef VectorN<double, 4> Vec4d;
typedef MatrixN<double, 2> Mat2d;
typedef MatrixN<double, 3> Mat3d;
typedef MatrixN<double, 4> Mat4d;
typedef VectorN<float, 2> Vec2f;
typedef VectorN<float, 3> Vec3f;
typedef VectorN<float, 4> Vec4f;
typedef MatrixN<float, 3> Mat3f;
typedef MatrixN<float, 4> Mat4f;
template<class T>
class Quaternion
{
public:
Vec3d v;
T w;
Quaternion () : v(0, 0, 0), w(0) {};
Quaternion (const Quaternion<T>& q) : v(q.v), w(q.w) {};
Quaternion (const Vec3d& vec, T _w) : v(vec), w(_w) {};
Quaternion (const Vec4d& vec) : v(vec[0], vec[1], vec[2]), w(vec[3]) {};
Quaternion (T _x, T _y, T _z, T _w) : v(_x, _y, _z), w(_w) {};
inline void reset () {
v.zeros();
w = (T) 0;
}
inline Quaternion<T>& operator = (const Quaternion<T>& rhs) {
v = rhs.v;
w = rhs.w;
return *this;
}
inline Quaternion<T> operator + (const Quaternion<T>& rhs) const {
const Quaternion<T>& lhs = *this;
return Quaternion<T>(lhs.v + rhs.v, lhs.w + rhs.w);
}
inline Quaternion<T> operator - (const Quaternion<T>& rhs) const {
const Quaternion<T>& lhs = *this;
return Quaternion<T>(lhs.v - rhs.v, lhs.w - rhs.w);
}
inline Quaternion<T> operator * (T rhs) const {
return Quaternion<T>(v * rhs, w * rhs);
}
inline Quaternion<T> operator * (const Quaternion<T>& rhs) const {
const Quaternion<T>& lhs = *this;
return Quaternion<T>(lhs.w * rhs.v[0] + lhs.v[0] * rhs.w + lhs.v[1] * rhs.v[2] - lhs.v[2] * rhs.v[1],
lhs.w * rhs.v[1] - lhs.v[0] * rhs.v[2] + lhs.v[1] * rhs.w + lhs.v[2] * rhs.v[0],
lhs.w * rhs.v[2] + lhs.v[0] * rhs.v[1] - lhs.v[1] * rhs.v[0] + lhs.v[2] * rhs.w,
lhs.w * rhs.w - lhs.v[0] * rhs.v[0] - lhs.v[1] * rhs.v[1] - lhs.v[2] * rhs.v[2]);
}
/*
--------
/ --
|Qr| = \/ Qr.Qr
*/
inline T magnitude () const {
return (T) sqrtf(w * w + v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
}
inline void normalize ()
{
T length = magnitude ();
w = w / length;
v = v / length;
}
inline Quaternion<T> conjugate (const Quaternion<T>& quat) const {
return Quaternion<T>(-quat.v, quat.w);
}
inline Quaternion<T> inverse (const Quaternion<T>& quat) const {
return conjugate(quat) * ( 1.0f / magnitude(quat));
}
inline Quaternion<T> lerp (T weight, const Quaternion<T>& quat) const {
return Quaternion<T>(v.lerp(weight, quat.v), (1 - weight) * w + weight * quat.w);
}
inline Quaternion<T> slerp(T r, const Quaternion<T>& quat) const {
Quaternion<T> ret;
T cos_theta = w * quat.w + v[0] * quat.v[0] + v[1] * quat.v[1] + v[2] * quat.v[2];
T theta = (T) acos(cos_theta);
if (fabs(theta) < FLT_EPSILON)
{
ret = *this;
}
else
{
T sin_theta = (T) sqrt(1.0 - cos_theta * cos_theta);
if (fabs(sin_theta) < FLT_EPSILON)
{
ret.w = 0.5 * w + 0.5 * quat.w;
ret.v = v.lerp(0.5, quat.v);
}
else
{
T r0 = (T) sin((1.0 - r) * theta) / sin_theta;
T r1 = (T) sin(r * theta) / sin_theta;
ret.w = w * r0 + quat.w * r1;
ret.v[0] = v[0] * r0 + quat.v[0] * r1;
ret.v[1] = v[1] * r0 + quat.v[1] * r1;
ret.v[2] = v[2] * r0 + quat.v[2] * r1;
}
}
return ret;
}
static Quaternion<T> create_quaternion (Vec3d axis, T angle_rad) {
T theta_over_two = angle_rad / (T) 2.0;
T sin_theta_over_two = std::sin(theta_over_two);
T cos_theta_over_two = std::cos(theta_over_two);
return Quaternion<T>(axis * sin_theta_over_two, cos_theta_over_two);
}
static Quaternion<T> create_quaternion (Vec3d euler) {
return create_quaternion(Vec3d(1, 0, 0), euler[0]) *
create_quaternion(Vec3d(0, 1, 0), euler[1]) *
create_quaternion(Vec3d(0, 0, 1), euler[2]);
}
static Quaternion<T> create_quaternion (const Mat3d& mat) {
Quaternion<T> q;
T trace, s;
T diag1 = mat(0, 0);
T diag2 = mat(1, 1);
T diag3 = mat(2, 2);
trace = diag1 + diag2 + diag3;
if (trace >= FLT_EPSILON)
{
s = 2.0 * (T) sqrt(trace + 1.0);
q.w = 0.25 * s;
q.v[0] = (mat(2, 1) - mat(1, 2)) / s;
q.v[1] = (mat(0, 2) - mat(2, 0)) / s;
q.v[2] = (mat(1, 0) - mat(0, 1)) / s;
}
else
{
char max_diag = (diag1 > diag2) ? ((diag1 > diag3) ? 1 : 3) : ((diag2 > diag3) ? 2 : 3);
if (max_diag == 1)
{
s = 2.0 * (T) sqrt(1.0 + mat(0, 0) - mat(1, 1) - mat(2, 2));
q.w = (mat(2, 1) - mat(1, 2)) / s;
q.v[0] = 0.25 * s;
q.v[1] = (mat(0, 1) + mat(1, 0)) / s;
q.v[2] = (mat(0, 2) + mat(2, 0)) / s;
}
else if (max_diag == 2)
{
s = 2.0 * (T) sqrt(1.0 + mat(1, 1) - mat(0, 0) - mat(2, 2));
q.w = (mat(0, 2) - mat(2, 0)) / s;
q.v[0] = (mat(0, 1) + mat(1, 0)) / s;
q.v[1] = 0.25 * s;
q.v[2] = (mat(1, 2) + mat(2, 1)) / s;
}
else
{
s = 2.0 * (T) sqrt(1.0 + mat(2, 2) - mat(0, 0) - mat(1, 1));
q.w = (mat(1, 0) - mat(0, 1)) / s;
q.v[0] = (mat(0, 2) + mat(2, 0)) / s;
q.v[1] = (mat(1, 2) + mat(2, 1)) / s;
q.v[2] = 0.25 * s;
}
}
return q;
}
inline Vec4d rotation_axis () {
Vec4d rot_axis;
T cos_theta_over_two = w;
rot_axis[4] = (T) std::acos( cos_theta_over_two ) * 2.0f;
T sin_theta_over_two = (T) sqrt( 1.0 - cos_theta_over_two * cos_theta_over_two );
if ( fabs( sin_theta_over_two ) < 0.0005 ) sin_theta_over_two = 1;
rot_axis[0] = v[0] / sin_theta_over_two;
rot_axis[1] = v[1] / sin_theta_over_two;
rot_axis[2] = v[2] / sin_theta_over_two;
return rot_axis;
}
/*
psi=atan2(2.*(Q(:,1).*Q(:,4)-Q(:,2).*Q(:,3)),(Q(:,4).^2-Q(:,1).^2-Q(:,2).^2+Q(:,3).^2));
theta=asin(2.*(Q(:,1).*Q(:,3)+Q(:,2).*Q(:,4)));
phi=atan2(2.*(Q(:,3).*Q(:,4)-Q(:,1).*Q(:,2)),(Q(:,4).^2+Q(:,1).^2-Q(:,2).^2-Q(:,3).^2));
*/
inline Vec3d euler_angles () {
Vec3d euler;
// atan2(2*(qx*qw-qy*qz) , qw2-qx2-qy2+qz2)
euler[0] = atan2(2 * (v[0] * w - v[1] * v[2]),
w * w - v[0] * v[0] - v[1] * v[1] + v[2] * v[2]);
// asin(2*(qx*qz + qy*qw)
euler[1] = asin(2 * (v[0] * v[2] + v[1] * w));
// atan2(2*(qz*qw- qx*qy) , qw2 + qx2 - qy2 - qz2)
euler[2] = atan2(2 * (v[2] * w - v[0] * v[1]),
w * w + v[0] * v[0] - v[1] * v[1] - v[2] * v[2]);
return euler;
}
inline Mat3d rotation_matrix () {
Mat3d mat;
T xx = v[0] * v[0];
T xy = v[0] * v[1];
T xz = v[0] * v[2];
T xw = v[0] * w;
T yy = v[1] * v[1];
T yz = v[1] * v[2];
T yw = v[1] * w;
T zz = v[2] * v[2];
T zw = v[2] * w;
mat(0, 0) = 1 - 2 * (yy + zz);
mat(0, 1) = 2 * (xy - zw);
mat(0, 2) = 2 * (xz + yw);
mat(1, 0) = 2 * (xy + zw);
mat(1, 1) = 1 - 2 * (xx + zz);
mat(1, 2) = 2 * (yz - xw);
mat(2, 0) = 2 * (xz - yw);
mat(2, 1) = 2 * (yz + xw);
mat(2, 2) = 1 - 2 * (xx + yy);
return mat;
}
};
typedef Quaternion<double> Quaternd;
}
#endif //XCAM_VECTOR_MATRIX_H