Structs continued
Piotr Dziwinski
parent
b5b9fdb680
commit
7369b10a87
|
|
@ -40,5 +40,5 @@ namespace Math
|
|||
//! Degrees to radians multiplier
|
||||
const float DEG_TO_RAD = 0.01745329251994329547f;
|
||||
//! Radians to degrees multiplier
|
||||
const FLOAT RAD_TO_DEG = 57.29577951308232286465f;
|
||||
const float RAD_TO_DEG = 57.29577951308232286465f;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
#pragma once
|
||||
|
||||
#include "const.h"
|
||||
#include "point.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
|
|
@ -28,10 +29,16 @@
|
|||
namespace Math
|
||||
{
|
||||
|
||||
//! Compares a and b within TOLERANCE
|
||||
bool IsEqual(float a, float b)
|
||||
//! Compares \a a and \a b within \a tolerance
|
||||
inline bool IsEqual(float a, float b, float tolerance = Math::TOLERANCE)
|
||||
{
|
||||
return Abs(a - b) < TOLERANCE;
|
||||
return fabs(a - b) < tolerance;
|
||||
}
|
||||
|
||||
//! Compares \a a to zero within \a tolerance
|
||||
inline bool IsZero(float a, float tolerance = Math::TOLERANCE)
|
||||
{
|
||||
return IsEqual(a, 0.0f, tolerance);
|
||||
}
|
||||
|
||||
//! Minimum
|
||||
|
|
@ -86,12 +93,6 @@ inline float Norm(float a)
|
|||
return a;
|
||||
}
|
||||
|
||||
//! Returns the absolute value
|
||||
inline float Abs(float a)
|
||||
{
|
||||
return (float)std::fabs(a);
|
||||
}
|
||||
|
||||
//! Swaps two integers
|
||||
inline void Swap(int &a, int &b)
|
||||
{
|
||||
|
|
@ -188,7 +189,7 @@ inline float Rand()
|
|||
out: -1 0 0 1 */
|
||||
float Neutral(float value, float dead)
|
||||
{
|
||||
if ( Abs(value) <= dead )
|
||||
if ( fabs(value) <= dead )
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
|
|
@ -252,3 +253,6 @@ inline float Bounce(float progress, float middle, float bounce)
|
|||
return (1.0f-bounce/2.0f)+sinf((0.5f+progress*2.0f)*PI)*(bounce/2.0f);
|
||||
}
|
||||
}
|
||||
|
||||
}; // namespace Math
|
||||
|
||||
|
|
|
|||
|
|
@ -21,8 +21,11 @@
|
|||
#pragma once
|
||||
|
||||
#include "const.h"
|
||||
#include "func.h"
|
||||
#include "vector.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <cassert>
|
||||
|
||||
// Math module namespace
|
||||
namespace Math
|
||||
|
|
@ -33,6 +36,18 @@ namespace Math
|
|||
Represents an universal 4x4 matrix that can be used in OpenGL and DirectX engines.
|
||||
Contains the required methods for operating on matrices (inverting, multiplying, etc.).
|
||||
|
||||
The internal representation is a 16-value table in column-major order, thus:
|
||||
|
||||
m[0 ] m[4 ] m[8 ] m[12]
|
||||
m[1 ] m[5 ] m[9 ] m[13]
|
||||
m[2 ] m[6 ] m[10] m[14]
|
||||
m[3 ] m[7 ] m[11] m[15]
|
||||
|
||||
This representation is native to OpenGL; DirectX requires transposing the matrix.
|
||||
|
||||
The order of multiplication of matrix and vector is also OpenGL-native
|
||||
(see the method Vector::MultiplyMatrix).
|
||||
|
||||
All methods are made inline to maximize optimization.
|
||||
|
||||
TODO test
|
||||
|
|
@ -40,7 +55,7 @@ namespace Math
|
|||
**/
|
||||
struct Matrix
|
||||
{
|
||||
//! Matrix values in row-major format
|
||||
//! Matrix values in column-major format
|
||||
float m[16];
|
||||
|
||||
//! Creates the indentity matrix
|
||||
|
|
@ -50,10 +65,11 @@ struct Matrix
|
|||
}
|
||||
|
||||
//! Creates the matrix from given values
|
||||
/** \a m values in row-major format */
|
||||
inline Matrix(float m[16])
|
||||
/** \a m values in column-major format */
|
||||
inline Matrix(const float (&m)[16])
|
||||
{
|
||||
this->m = m;
|
||||
for (int i = 0; i < 16; ++i)
|
||||
this->m[i] = m[i];
|
||||
}
|
||||
|
||||
//! Loads the zero matrix
|
||||
|
|
@ -70,20 +86,8 @@ struct Matrix
|
|||
m[0] = m[5] = m[10] = m[15] = 1.0f;
|
||||
}
|
||||
|
||||
//! Calculates the determinant of the matrix
|
||||
/** \returns the determinant */
|
||||
float Det() const
|
||||
{
|
||||
float result = 0.0f;
|
||||
for (int i = 0; i < 4; ++i)
|
||||
{
|
||||
result += m[0][i] * Cofactor(0, i);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
//! Transposes the matrix
|
||||
void Transpose()
|
||||
inline void Transpose()
|
||||
{
|
||||
Matrix temp = *this;
|
||||
for (int r = 0; r < 4; ++r)
|
||||
|
|
@ -95,37 +99,197 @@ struct Matrix
|
|||
}
|
||||
}
|
||||
|
||||
//! Calculates the determinant of the matrix
|
||||
/** \returns the determinant */
|
||||
inline float Det() const
|
||||
{
|
||||
float result = 0.0f;
|
||||
for (int i = 0; i < 4; ++i)
|
||||
{
|
||||
result += m[i] * Cofactor(i, 0);
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
//! Calculates the cofactor of the matrix
|
||||
/** \a r row (0 to 3)
|
||||
\a c column (0 to 3)
|
||||
\returns the cofactor or 0.0f if invalid r, c given*/
|
||||
float Cofactor(int r, int c) const
|
||||
inline float Cofactor(int r, int c) const
|
||||
{
|
||||
if ((r < 0) || (r > 3) || (c < 0) || (c > 3))
|
||||
return 0.0f;
|
||||
assert(r >= 0 && r <= 3);
|
||||
assert(c >= 0 && c <= 3);
|
||||
|
||||
float tab[3][3];
|
||||
int tabR = 0;
|
||||
for (int r = 0; r < 4; ++r)
|
||||
float result = 0.0f;
|
||||
|
||||
/* That looks horrible, I know. But it's fast :) */
|
||||
|
||||
switch (4*r + c)
|
||||
{
|
||||
if (r == i) continue;
|
||||
int tabC = 0;
|
||||
for (int c = 0; c < 4; ++c)
|
||||
{
|
||||
if (c == j) continue;
|
||||
tab[tabR][tabC] = m[4*r + c];
|
||||
++tabC;
|
||||
}
|
||||
++tabR;
|
||||
// r=0, c=0
|
||||
/* 05 09 13
|
||||
06 10 14
|
||||
07 11 15 */
|
||||
case 0:
|
||||
result = + m[5 ] * (m[10] * m[15] - m[14] * m[11])
|
||||
- m[9 ] * (m[6 ] * m[15] - m[14] * m[7 ])
|
||||
+ m[13] * (m[6 ] * m[11] - m[10] * m[7 ]);
|
||||
break;
|
||||
|
||||
// r=0, c=1
|
||||
/* 01 09 13
|
||||
02 10 14
|
||||
03 11 15 */
|
||||
case 1:
|
||||
result = - m[1 ] * (m[10] * m[15] - m[14] * m[11])
|
||||
+ m[9 ] * (m[2 ] * m[15] - m[14] * m[3 ])
|
||||
- m[13] * (m[2 ] * m[11] - m[10] * m[3 ]);
|
||||
break;
|
||||
|
||||
// r=0, c=2
|
||||
/* 01 05 13
|
||||
02 06 14
|
||||
03 07 15 */
|
||||
case 2:
|
||||
result = + m[1 ] * (m[6 ] * m[15] - m[14] * m[7 ])
|
||||
- m[5 ] * (m[2 ] * m[15] - m[14] * m[3 ])
|
||||
+ m[13] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]);
|
||||
break;
|
||||
|
||||
// r=0, c=3
|
||||
/* 01 05 09
|
||||
02 06 10
|
||||
03 07 11 */
|
||||
case 3:
|
||||
result = - m[1 ] * (m[6 ] * m[11] - m[10] * m[7 ])
|
||||
+ m[5 ] * (m[2 ] * m[11] - m[10] * m[3 ])
|
||||
- m[9 ] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]);
|
||||
break;
|
||||
|
||||
// r=1, c=0
|
||||
/* 04 08 12
|
||||
06 10 14
|
||||
07 11 15 */
|
||||
case 4:
|
||||
result = - m[4 ] * (m[10] * m[15] - m[14] * m[11])
|
||||
+ m[8 ] * (m[6 ] * m[15] - m[14] * m[7 ])
|
||||
- m[12] * (m[6 ] * m[11] - m[10] * m[7 ]);
|
||||
break;
|
||||
|
||||
// r=1, c=1
|
||||
/* 00 08 12
|
||||
02 10 14
|
||||
03 11 15 */
|
||||
case 5:
|
||||
result = + m[0 ] * (m[10] * m[15] - m[14] * m[11])
|
||||
- m[8 ] * (m[2 ] * m[15] - m[14] * m[3 ])
|
||||
+ m[12] * (m[2 ] * m[11] - m[10] * m[3 ]);
|
||||
break;
|
||||
|
||||
// r=1, c=2
|
||||
/* 00 04 12
|
||||
02 06 14
|
||||
03 07 15 */
|
||||
case 6:
|
||||
result = - m[0 ] * (m[6 ] * m[15] - m[14] * m[7 ])
|
||||
+ m[4 ] * (m[2 ] * m[15] - m[14] * m[3 ])
|
||||
- m[12] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]);
|
||||
break;
|
||||
|
||||
// r=1, c=3
|
||||
/* 00 04 08
|
||||
02 06 10
|
||||
03 07 11 */
|
||||
case 7:
|
||||
result = + m[0 ] * (m[6 ] * m[11] - m[10] * m[7 ])
|
||||
- m[4 ] * (m[2 ] * m[11] - m[10] * m[3 ])
|
||||
+ m[8 ] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]);
|
||||
break;
|
||||
|
||||
// r=2, c=0
|
||||
/* 04 08 12
|
||||
05 09 13
|
||||
07 11 15 */
|
||||
case 8:
|
||||
result = + m[4 ] * (m[9 ] * m[15] - m[13] * m[11])
|
||||
- m[8 ] * (m[5 ] * m[15] - m[13] * m[7 ])
|
||||
+ m[12] * (m[5 ] * m[11] - m[9 ] * m[7 ]);
|
||||
break;
|
||||
|
||||
// r=2, c=1
|
||||
/* 00 08 12
|
||||
01 09 13
|
||||
03 11 15 */
|
||||
case 9:
|
||||
result = - m[0 ] * (m[9 ] * m[15] - m[13] * m[11])
|
||||
+ m[8 ] * (m[1 ] * m[15] - m[13] * m[3 ])
|
||||
- m[12] * (m[1 ] * m[11] - m[9 ] * m[3 ]);
|
||||
break;
|
||||
|
||||
// r=2, c=2
|
||||
/* 00 04 12
|
||||
01 05 13
|
||||
03 07 15 */
|
||||
case 10:
|
||||
result = + m[0 ] * (m[5 ] * m[15] - m[13] * m[7 ])
|
||||
- m[4 ] * (m[1 ] * m[15] - m[13] * m[3 ])
|
||||
+ m[12] * (m[1 ] * m[7 ] - m[5 ] * m[3 ]);
|
||||
break;
|
||||
|
||||
// r=2, c=3
|
||||
/* 00 04 08
|
||||
01 05 09
|
||||
03 07 11 */
|
||||
case 11:
|
||||
result = - m[0 ] * (m[5 ] * m[11] - m[9 ] * m[7 ])
|
||||
+ m[4 ] * (m[1 ] * m[11] - m[9 ] * m[3 ])
|
||||
- m[8 ] * (m[1 ] * m[7 ] - m[5 ] * m[3 ]);
|
||||
break;
|
||||
|
||||
// r=3, c=0
|
||||
/* 04 08 12
|
||||
05 09 13
|
||||
06 10 14 */
|
||||
case 12:
|
||||
result = - m[4 ] * (m[9 ] * m[14] - m[13] * m[10])
|
||||
+ m[8 ] * (m[5 ] * m[14] - m[13] * m[6 ])
|
||||
- m[12] * (m[5 ] * m[10] - m[9 ] * m[6 ]);
|
||||
break;
|
||||
|
||||
// r=3, c=1
|
||||
/* 00 08 12
|
||||
01 09 13
|
||||
02 10 14 */
|
||||
case 13:
|
||||
result = + m[0 ] * (m[9 ] * m[14] - m[13] * m[10])
|
||||
- m[8 ] * (m[1 ] * m[14] - m[13] * m[2 ])
|
||||
+ m[12] * (m[1 ] * m[10] - m[9 ] * m[2 ]);
|
||||
break;
|
||||
|
||||
// r=3, c=2
|
||||
/* 00 04 12
|
||||
01 05 13
|
||||
02 06 14 */
|
||||
case 14:
|
||||
result = - m[0 ] * (m[5 ] * m[14] - m[13] * m[6 ])
|
||||
+ m[4 ] * (m[1 ] * m[14] - m[13] * m[2 ])
|
||||
- m[12] * (m[1 ] * m[6 ] - m[5 ] * m[2 ]);
|
||||
break;
|
||||
|
||||
// r=3, c=3
|
||||
/* 00 04 08
|
||||
01 05 09
|
||||
02 06 10 */
|
||||
case 15:
|
||||
result = + m[0 ] * (m[5 ] * m[10] - m[9 ] * m[6 ])
|
||||
- m[4 ] * (m[1 ] * m[10] - m[9 ] * m[2 ])
|
||||
+ m[8 ] * (m[1 ] * m[6 ] - m[5 ] * m[2 ]);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
float result = tab[0][0] * (tab[1][1] * tab[2][2] - tab[1][2] * tab[2][1])
|
||||
- tab[0][1] * (tab[1][0] * tab[2][2] - tab[1][2] * tab[2][0])
|
||||
+ tab[0][2] * (tab[1][0] * tab[2][1] - tab[1][1] * tab[2][0]);
|
||||
|
||||
if ((i + j) % 2 == 0)
|
||||
result = -result;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
@ -133,19 +297,16 @@ struct Matrix
|
|||
inline void Invert()
|
||||
{
|
||||
float d = Det();
|
||||
if (fabs(d) <= Math::TOLERANCE)
|
||||
return;
|
||||
assert(! IsZero(d));
|
||||
|
||||
Matrix temp = *this;
|
||||
for (int r = 0; r < 4; ++r)
|
||||
{
|
||||
for (int c = 0; c < 4; ++c)
|
||||
{
|
||||
m[r][c] = (1.0f / d) * temp.Cofactor(r, c);
|
||||
m[4*r+c] = (1.0f / d) * temp.Cofactor(r, c);
|
||||
}
|
||||
}
|
||||
|
||||
Tranpose();
|
||||
}
|
||||
|
||||
//! Multiplies the matrix with the given matrix
|
||||
|
|
@ -153,14 +314,14 @@ struct Matrix
|
|||
inline void Multiply(const Matrix &right)
|
||||
{
|
||||
Matrix left = *this;
|
||||
for (int r = 0; r < 4; ++r)
|
||||
for (int c = 0; c < 4; ++c)
|
||||
{
|
||||
for (int c = 0; c < 4; ++c)
|
||||
for (int r = 0; r < 4; ++r)
|
||||
{
|
||||
m[r][c] = 0.0;
|
||||
m[4*c+r] = 0.0f;
|
||||
for (int i = 0; i < 4; ++i)
|
||||
{
|
||||
m[4*r+c] += left.m[4*r+i] * right.m[4*i+c];
|
||||
m[4*c+r] += left.m[4*i+r] * right.m[4*c+i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -168,17 +329,16 @@ struct Matrix
|
|||
|
||||
//! Loads view matrix from the given vectors
|
||||
/** \a from origin
|
||||
\a at direction
|
||||
\a up up vector */
|
||||
inline void LoadView(const Vector &from, const Vector &at, const Vector &up)
|
||||
\a at view direction
|
||||
\a worldUp up vector */
|
||||
inline void LoadView(const Vector &from, const Vector &at, const Vector &worldUp)
|
||||
{
|
||||
// Get the z basis vector, which points straight ahead. This is the
|
||||
// difference from the eyepoint to the lookat point.
|
||||
Vector view = at - from;
|
||||
|
||||
FLOAT length = view.Length();
|
||||
if( IsZero(length) )
|
||||
return;
|
||||
float length = view.Length();
|
||||
assert(! Math::IsZero(length) );
|
||||
|
||||
// Normalize the z basis vector
|
||||
view /= length;
|
||||
|
|
@ -200,8 +360,7 @@ struct Matrix
|
|||
{
|
||||
up = Vector(0.0f, 0.0f, 1.0f) - view.z * view;
|
||||
|
||||
if ( IsZero(up.Length()) )
|
||||
return;
|
||||
assert(! IsZero(up.Length()) );
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -220,9 +379,9 @@ struct Matrix
|
|||
m[8 ] = right.z; m[9 ] = up.z; m[10] = view.z;
|
||||
|
||||
// Do the translation values (rotations are still about the eyepoint)
|
||||
m[3 ] = - DotProduct(from, right);
|
||||
m[7 ] = - DotProduct(from, up);
|
||||
m[11] = - DotProduct(from, view);
|
||||
m[12] = - DotProduct(from, right);
|
||||
m[13] = - DotProduct(from, up);
|
||||
m[14] = - DotProduct(from, view);
|
||||
}
|
||||
|
||||
inline void LoadProjection(float fov = 1.570795f, float aspect = 1.0f,
|
||||
|
|
@ -233,12 +392,18 @@ struct Matrix
|
|||
|
||||
inline void LoadTranslation(const Vector &trans)
|
||||
{
|
||||
// TODO
|
||||
LoadIdentity();
|
||||
m[12] = trans.x;
|
||||
m[13] = trans.y;
|
||||
m[14] = trans.z;
|
||||
}
|
||||
|
||||
inline void LoadScale(const Vector &scale)
|
||||
{
|
||||
// TODO
|
||||
LoadIdentity();
|
||||
m[0] = scale.x;
|
||||
m[5] = scale.y;
|
||||
m[10] = scale.z;
|
||||
}
|
||||
|
||||
inline void LoadRotationX(float angle)
|
||||
|
|
@ -264,22 +429,26 @@ struct Matrix
|
|||
//! Calculates the matrix to make three rotations in the order X, Z and Y
|
||||
inline void RotateXZY(const Vector &angle)
|
||||
{
|
||||
this->LoadRotationX(angle.x);
|
||||
|
||||
Matrix temp;
|
||||
temp.SetRotateXMatrix(angle.x);
|
||||
this->SetRotateZMatrix(angle.z);
|
||||
temp.LoadRotationZ(angle.z);
|
||||
this->Multiply(temp);
|
||||
temp.SetRotateYMatrix(angle.y);
|
||||
|
||||
temp.LoadRotationY(angle.y);
|
||||
this->Multiply(temp);
|
||||
}
|
||||
|
||||
//! Calculates the matrix to make three rotations in the order Z, X and Y
|
||||
inline void RotateZXY(const Vector &angle)
|
||||
{
|
||||
this->LoadRotationZ(angle.z);
|
||||
|
||||
Matrix temp;
|
||||
temp.SetRotateZMatrix(angle.z);
|
||||
this->SetRotateXMatrix(angle.x);
|
||||
temp.LoadRotationX(angle.x);
|
||||
this->Multiply(temp);
|
||||
temp.SetRotateYMatrix(angle.y);
|
||||
|
||||
temp.LoadRotationY(angle.y);
|
||||
this->Multiply(temp);
|
||||
}
|
||||
};
|
||||
|
|
@ -287,7 +456,7 @@ struct Matrix
|
|||
//! Convenience function for inverting a matrix
|
||||
/** \a m input matrix
|
||||
\a result result -- inverted matrix */
|
||||
void InvertMatrix(const Matrix &m, Matrix &result)
|
||||
inline void InvertMatrix(const Matrix &m, Matrix &result)
|
||||
{
|
||||
result = m;
|
||||
result.Invert();
|
||||
|
|
@ -296,11 +465,42 @@ void InvertMatrix(const Matrix &m, Matrix &result)
|
|||
//! Convenience function for multiplying a matrix
|
||||
/** \a left left-hand matrix
|
||||
\a right right-hand matrix
|
||||
\a result result -- multiplied matrices */*
|
||||
void MultiplyMatrices(const Matrix &left, const Matrix &right, Matrix &result)
|
||||
\a result result -- multiplied matrices */
|
||||
inline void MultiplyMatrices(const Matrix &left, const Matrix &right, Matrix &result)
|
||||
{
|
||||
result = left;
|
||||
result.Multiply(right);
|
||||
}
|
||||
|
||||
//! Calculates the result of multiplying m * v
|
||||
inline Vector MatrixVectorMultiply(const Matrix &m, const Vector &v)
|
||||
{
|
||||
float x = v.x * m.m[0 ] + v.y * m.m[4 ] + v.z * m.m[8 ] + m.m[12];
|
||||
float y = v.x * m.m[1 ] + v.y * m.m[5 ] + v.z * m.m[9 ] + m.m[13];
|
||||
float z = v.x * m.m[2 ] + v.y * m.m[6 ] + v.z * m.m[10] + m.m[14];
|
||||
float w = v.x * m.m[4 ] + v.y * m.m[7 ] + v.z * m.m[11] + m.m[15];
|
||||
|
||||
if (IsZero(w))
|
||||
return Vector(x, y, z);
|
||||
|
||||
x /= w;
|
||||
y /= w;
|
||||
z /= w;
|
||||
|
||||
return Vector(x, y, z);
|
||||
}
|
||||
|
||||
//! Checks if two matrices are equal within given \a tolerance
|
||||
inline bool MatricesEqual(const Math::Matrix &m1, const Math::Matrix &m2,
|
||||
float tolerance = Math::TOLERANCE)
|
||||
{
|
||||
for (int i = 0; i < 16; ++i)
|
||||
{
|
||||
if (! IsEqual(m1.m[i], m2.m[i], tolerance))
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
}; // namespace Math
|
||||
|
|
|
|||
|
|
@ -0,0 +1,14 @@
|
|||
cmake_minimum_required(VERSION 2.8)
|
||||
|
||||
set(CMAKE_BUILD_TYPE debug)
|
||||
set(CMAKE_CXX_FLAGS_DEBUG "-Wall -g -O0")
|
||||
|
||||
add_executable(matrix_test matrix_test.cpp)
|
||||
|
||||
enable_testing()
|
||||
|
||||
add_test(matrix_test ./matrix_test)
|
||||
|
||||
add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} DEPENDS matrix_test)
|
||||
|
||||
add_custom_target(distclean COMMAND rm -rf ./matrix_test CMakeFiles Testing cmake_install.cmake CMakeCache.txt CTestTestfile.cmake Makefile)
|
||||
|
|
@ -0,0 +1,86 @@
|
|||
% Script in Octave for generating test data
|
||||
|
||||
1;
|
||||
|
||||
% Returns the minor matrix
|
||||
function m = minor(A, r, c)
|
||||
|
||||
m = A;
|
||||
m(r,:) = [];
|
||||
m(:,c) = [];
|
||||
|
||||
end;
|
||||
|
||||
% Returns the cofactor matrix
|
||||
function m = cofactors(A)
|
||||
|
||||
m = zeros(rows(A), columns(A));
|
||||
|
||||
for r = [1 : rows(A)]
|
||||
for c = [1 : columns(A)]
|
||||
m(r, c) = det(minor(A, r, c));
|
||||
if (mod(r + c, 2) == 1)
|
||||
m(r, c) = -m(r, c);
|
||||
end;
|
||||
end;
|
||||
end;
|
||||
|
||||
end;
|
||||
|
||||
% Prints the matrix as C++ code
|
||||
function printout(A, name)
|
||||
|
||||
printf('const float %s[16] = \n', name);
|
||||
printf('{\n');
|
||||
|
||||
for c = [1 : columns(A)]
|
||||
for r = [1 : rows(A)]
|
||||
printf(' %f', A(r,c));
|
||||
if (! ( (r == 4) && (c == 4) ) )
|
||||
printf(',');
|
||||
end;
|
||||
printf('\n');
|
||||
end;
|
||||
end;
|
||||
|
||||
printf('};\n');
|
||||
|
||||
end;
|
||||
|
||||
printf('// Cofactors\n');
|
||||
A = randn(4,4);
|
||||
printout(A, 'COF_MAT');
|
||||
printf('\n');
|
||||
printout(cofactors(A), 'COF_RESULT');
|
||||
printf('\n');
|
||||
|
||||
printf('\n');
|
||||
|
||||
printf('// Det\n');
|
||||
A = randn(4,4);
|
||||
printout(A, 'DET_MAT');
|
||||
printf('\n');
|
||||
printf('const float DET_RESULT = %f;', det(A));
|
||||
printf('\n');
|
||||
|
||||
printf('\n');
|
||||
|
||||
printf('// Invert\n');
|
||||
A = randn(4,4);
|
||||
printout(A, 'INV_MAT');
|
||||
printf('\n');
|
||||
printout(inv(A), 'COF_RESULT');
|
||||
printf('\n');
|
||||
|
||||
printf('\n');
|
||||
|
||||
printf('// Multiplication\n');
|
||||
A = randn(4,4);
|
||||
printout(A, 'MUL_A');
|
||||
printf('\n');
|
||||
B = randn(4,4);
|
||||
printout(B, 'MUL_B');
|
||||
printf('\n');
|
||||
C = A * B;
|
||||
printout(C, 'MUL_RESULT');
|
||||
printf('\n');
|
||||
|
|
@ -0,0 +1,287 @@
|
|||
// * This file is part of the COLOBOT source code
|
||||
// * Copyright (C) 2012, Polish Portal of Colobot (PPC)
|
||||
// *
|
||||
// * This program is free software: you can redistribute it and/or modify
|
||||
// * it under the terms of the GNU General Public License as published by
|
||||
// * the Free Software Foundation, either version 3 of the License, or
|
||||
// * (at your option) any later version.
|
||||
// *
|
||||
// * This program is distributed in the hope that it will be useful,
|
||||
// * but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
// * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
// * GNU General Public License for more details.
|
||||
// *
|
||||
// * You should have received a copy of the GNU General Public License
|
||||
// * along with this program. If not, see http://www.gnu.org/licenses/.
|
||||
|
||||
// math/test/matrix_test.cpp
|
||||
|
||||
/* Unit tests for Matrix struct
|
||||
|
||||
Test data was randomly generated and the expected
|
||||
results calculated using GNU Octave.
|
||||
|
||||
*/
|
||||
|
||||
#include "../func.h"
|
||||
#include "../matrix.h"
|
||||
|
||||
#include <cstdio>
|
||||
|
||||
using namespace std;
|
||||
|
||||
const float TEST_TOLERANCE = 1e-5;
|
||||
|
||||
int TestCofactor()
|
||||
{
|
||||
const float TEST_MATRIX[16] =
|
||||
{
|
||||
-0.306479,
|
||||
-0.520207,
|
||||
0.127906,
|
||||
0.632922,
|
||||
|
||||
-0.782876,
|
||||
0.015264,
|
||||
0.337479,
|
||||
1.466013,
|
||||
|
||||
0.072725,
|
||||
-0.315123,
|
||||
1.613198,
|
||||
-0.577377,
|
||||
|
||||
0.962397,
|
||||
-1.320724,
|
||||
1.467588,
|
||||
0.579020
|
||||
};
|
||||
|
||||
const float EXPECTED_RESULTS[4][4] =
|
||||
{
|
||||
{ 2.791599, -0.249952, 1.065075, -1.356570 },
|
||||
{ 3.715943, -1.537511, 0.094812, -0.074520 },
|
||||
{ 1.034500, -0.731752, -0.920756, -0.196235 },
|
||||
{ 1.213928, -1.236857, 0.779741, -0.678482 }
|
||||
};
|
||||
|
||||
Math::Matrix mat(TEST_MATRIX);
|
||||
|
||||
for (int r = 0; r < 4; ++r)
|
||||
{
|
||||
for (int c = 0; c < 4; ++c)
|
||||
{
|
||||
float ret = mat.Cofactor(r, c);
|
||||
float exp = EXPECTED_RESULTS[r][c];
|
||||
if (! Math::IsEqual(ret, exp, TEST_TOLERANCE))
|
||||
{
|
||||
fprintf(stderr, "Cofactor r=%d, c=%d, %f (returned) != %f (expected)\n", r, c, ret, exp);
|
||||
return 4*c+r;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int TestDet()
|
||||
{
|
||||
const float TEST_MATRIX[16] =
|
||||
{
|
||||
0.85554,
|
||||
0.11624,
|
||||
1.30411,
|
||||
0.81467,
|
||||
|
||||
0.49692,
|
||||
-1.92483,
|
||||
-1.33543,
|
||||
0.85042,
|
||||
|
||||
-0.16775,
|
||||
0.35344,
|
||||
1.40673,
|
||||
0.13961,
|
||||
|
||||
1.40709,
|
||||
0.11731,
|
||||
0.69042,
|
||||
0.91216
|
||||
};
|
||||
|
||||
const float EXPECTED_RESULT = 0.084360;
|
||||
|
||||
float ret = Math::Matrix(TEST_MATRIX).Det();
|
||||
if (! Math::IsEqual(ret, EXPECTED_RESULT, TEST_TOLERANCE))
|
||||
{
|
||||
fprintf(stderr, "Det %f (returned) != %f (expected)\n", ret, EXPECTED_RESULT);
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int TestInvert()
|
||||
{
|
||||
const float TEST_MATRIX[16] =
|
||||
{
|
||||
1.4675123,
|
||||
-0.2857923,
|
||||
-0.0496217,
|
||||
-1.2825408,
|
||||
|
||||
-0.2804135,
|
||||
-0.0826255,
|
||||
-0.6825495,
|
||||
1.1661259,
|
||||
|
||||
0.0032798,
|
||||
0.5999200,
|
||||
-1.8359883,
|
||||
-1.1894424,
|
||||
|
||||
-1.1501538,
|
||||
-2.8792485,
|
||||
0.0299345,
|
||||
0.3730919
|
||||
};
|
||||
|
||||
const float EXPECTED_RESULT[16] =
|
||||
{
|
||||
0.685863,
|
||||
0.562274,
|
||||
-0.229722,
|
||||
-0.132079,
|
||||
|
||||
-0.266333,
|
||||
-0.139862,
|
||||
0.054211,
|
||||
-0.305568,
|
||||
|
||||
-0.130817,
|
||||
-0.494076,
|
||||
-0.358226,
|
||||
-0.047477,
|
||||
|
||||
0.069486,
|
||||
0.693649,
|
||||
-0.261074,
|
||||
-0.081200
|
||||
};
|
||||
|
||||
Math::Matrix mat(TEST_MATRIX);
|
||||
mat.Invert();
|
||||
|
||||
if (! Math::MatricesEqual(mat, EXPECTED_RESULT, TEST_TOLERANCE))
|
||||
{
|
||||
fprintf(stderr, "Invert mismatch\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int TestMultiply()
|
||||
{
|
||||
const float TEST_MATRIX_A[16] =
|
||||
{
|
||||
-1.931420,
|
||||
0.843410,
|
||||
0.476929,
|
||||
-0.493435,
|
||||
1.425659,
|
||||
-0.176331,
|
||||
0.129096,
|
||||
0.551081,
|
||||
-0.543530,
|
||||
-0.190783,
|
||||
-0.084744,
|
||||
1.379547,
|
||||
-0.473377,
|
||||
1.643398,
|
||||
0.400539,
|
||||
0.702937
|
||||
};
|
||||
|
||||
const float TEST_MATRIX_B[16] =
|
||||
{
|
||||
0.3517561,
|
||||
1.3903778,
|
||||
-0.8048254,
|
||||
-0.4090024,
|
||||
|
||||
-1.5542159,
|
||||
-0.6798636,
|
||||
1.6003393,
|
||||
-0.1467117,
|
||||
|
||||
0.5043620,
|
||||
-0.0068779,
|
||||
2.0697285,
|
||||
-0.0463650,
|
||||
|
||||
0.9605451,
|
||||
-0.4620149,
|
||||
1.2525952,
|
||||
-1.3409909
|
||||
};
|
||||
|
||||
const float EXPECTED_RESULT[16] =
|
||||
{
|
||||
1.933875,
|
||||
-0.467099,
|
||||
0.251638,
|
||||
-0.805156,
|
||||
|
||||
1.232207,
|
||||
-1.737383,
|
||||
-1.023401,
|
||||
2.496859,
|
||||
|
||||
-2.086953,
|
||||
-0.044468,
|
||||
0.045688,
|
||||
2.570036,
|
||||
|
||||
-2.559921,
|
||||
-1.551155,
|
||||
-0.244802,
|
||||
0.056808
|
||||
};
|
||||
|
||||
Math::Matrix matA(TEST_MATRIX_A);
|
||||
Math::Matrix matB(TEST_MATRIX_B);
|
||||
|
||||
Math::Matrix mat;
|
||||
Math::MultiplyMatrices(matA, matB, mat);
|
||||
if (! Math::MatricesEqual(mat, Math::Matrix(EXPECTED_RESULT), TEST_TOLERANCE ) )
|
||||
{
|
||||
fprintf(stderr, "Multiply mismath!\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int main()
|
||||
{
|
||||
int result = 0;
|
||||
|
||||
result = TestCofactor();
|
||||
if (result != 0)
|
||||
return result;
|
||||
|
||||
result = TestDet();
|
||||
if (result != 0)
|
||||
return result;
|
||||
|
||||
result = TestInvert();
|
||||
if (result != 0)
|
||||
return result;
|
||||
|
||||
result = TestMultiply();
|
||||
if (result != 0)
|
||||
return result;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
|
@ -21,6 +21,7 @@
|
|||
#pragma once
|
||||
|
||||
#include "const.h"
|
||||
#include "func.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
|
|
@ -85,16 +86,16 @@ struct Vector
|
|||
this->z = z;
|
||||
}
|
||||
|
||||
//! Loads the zero vector (0, 0, 0, 0)
|
||||
//! Loads the zero vector (0, 0, 0)
|
||||
inline void LoadZero()
|
||||
{
|
||||
x = y = z = w = 0.0f;
|
||||
x = y = z = 0.0f;
|
||||
}
|
||||
|
||||
//! Returns the vector length
|
||||
inline float Length() const
|
||||
{
|
||||
return sqrt(x*x + y*y + z*z + w*w);
|
||||
return sqrt(x*x + y*y + z*z);
|
||||
}
|
||||
|
||||
//! Normalizes the vector
|
||||
|
|
@ -104,12 +105,11 @@ struct Vector
|
|||
x /= l;
|
||||
y /= l;
|
||||
z /= l;
|
||||
w /= l;
|
||||
}
|
||||
|
||||
//! Calculates the cross product with another vector
|
||||
/** \a right right-hand side vector */
|
||||
inline void Cross(const Vector &right)
|
||||
inline void CrossMultiply(const Vector &right)
|
||||
{
|
||||
Vector left = *this;
|
||||
x = left.y * right.z - left.z * right.y;
|
||||
|
|
@ -118,7 +118,7 @@ struct Vector
|
|||
}
|
||||
|
||||
//! Calculates the dot product with another vector
|
||||
inline float Dot(const Vector &right) const
|
||||
inline float DotMultiply(const Vector &right) const
|
||||
{
|
||||
return x * right.x + y * right.y + z * right.z;
|
||||
}
|
||||
|
|
@ -126,7 +126,7 @@ struct Vector
|
|||
//! Returns the cosine of angle between this and another vector
|
||||
inline float CosAngle(const Vector &right) const
|
||||
{
|
||||
return Dot(right) / (Length() * right.Length());
|
||||
return DotMultiply(right) / (Length() * right.Length());
|
||||
}
|
||||
|
||||
//! Returns angle (in radians) between this and another vector
|
||||
|
|
@ -135,34 +135,90 @@ struct Vector
|
|||
return acos(CosAngle(right));
|
||||
}
|
||||
|
||||
//! Calculates the result of multiplying m * this vector
|
||||
inline MatrixMultiply(const Matrix &m)
|
||||
//! Returns the inverted vector
|
||||
inline Vector operator-() const
|
||||
{
|
||||
float tx = x * m.m[0 ] + y * m.m[4 ] + z * m.m[8 ] + m.m[12];
|
||||
float ty = x * m.m[1 ] + y * m.m[5 ] + z * m.m[9 ] + m.m[13];
|
||||
float tz = x * m.m[2 ] + y * m.m[6 ] + z * m.m[10] + m.m[14];
|
||||
float tw = x * m.m[4 ] + y * m.m[7 ] + z * m.m[11] + m.m[15];
|
||||
return Vector(-x, -y, -z);
|
||||
}
|
||||
|
||||
if (IsZero(tw))
|
||||
return;
|
||||
//! Adds the given vector
|
||||
inline const Vector& operator+=(const Vector &right)
|
||||
{
|
||||
x += right.x;
|
||||
y += right.y;
|
||||
z += right.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
x = tx / tw;
|
||||
y = ty / tw;
|
||||
z = tz / tw;
|
||||
//! Adds two vectors
|
||||
inline friend const Vector operator+(const Vector &left, const Vector &right)
|
||||
{
|
||||
return Vector(left.x + right.x, left.y + right.y, left.z + right.z);
|
||||
}
|
||||
|
||||
//! Subtracts the given vector
|
||||
inline const Vector& operator-=(const Vector &right)
|
||||
{
|
||||
x -= right.x;
|
||||
y -= right.y;
|
||||
z -= right.z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
//! Subtracts two vectors
|
||||
inline friend const Vector operator-(const Vector &left, const Vector &right)
|
||||
{
|
||||
return Vector(left.x - right.x, left.y - right.y, left.z - right.z);
|
||||
}
|
||||
|
||||
//! Multiplies by given scalar
|
||||
inline const Vector& operator*=(const float &right)
|
||||
{
|
||||
x *= right;
|
||||
y *= right;
|
||||
z *= right;
|
||||
return *this;
|
||||
}
|
||||
|
||||
//! Multiplies vector by scalar
|
||||
inline friend const Vector operator*(const float &left, const Vector &right)
|
||||
{
|
||||
return Vector(left * right.x, left * right.y, left * right.z);
|
||||
}
|
||||
|
||||
//! Multiplies vector by scalar
|
||||
inline friend const Vector operator*(const Vector &left, const float &right)
|
||||
{
|
||||
return Vector(left.x * right, left.y * right, left.z * right);
|
||||
}
|
||||
|
||||
//! Divides by given scalar
|
||||
inline const Vector& operator/=(const float &right)
|
||||
{
|
||||
x /= right;
|
||||
y /= right;
|
||||
z /= right;
|
||||
return *this;
|
||||
}
|
||||
|
||||
//! Divides vector by scalar
|
||||
inline friend const Vector operator/(const Vector &left, const float &right)
|
||||
{
|
||||
return Vector(left.x / right, left.y / right, left.z / right);
|
||||
}
|
||||
};
|
||||
|
||||
//! Convenience function for calculating dot product
|
||||
float DotProduct(const Vector &left, const Vector &right)
|
||||
inline float DotProduct(const Vector &left, const Vector &right)
|
||||
{
|
||||
return left.DotProduct(right);
|
||||
return left.DotMultiply(right);
|
||||
}
|
||||
|
||||
//! Convenience function for calculating cross product
|
||||
Vector CrossProduct(const Vector &left, const Vector &right)
|
||||
inline Vector CrossProduct(const Vector &left, const Vector &right)
|
||||
{
|
||||
Vector result = left;
|
||||
result.CrossProduct(right);
|
||||
result.CrossMultiply(right);
|
||||
return result;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue