Import all sources from perforce.
This commit is contained in:
castano
130
src/nvmath/Box.h
Normal file
130
src/nvmath/Box.h
Normal file
@ -0,0 +1,130 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_BOX_H
|
||||
#define NV_MATH_BOX_H
|
||||
|
||||
#include <nvmath/Vector.h>
|
||||
|
||||
#include <float.h> // FLT_MAX
|
||||
|
||||
namespace nv
|
||||
{
|
||||
|
||||
/// Axis Aligned Bounding Box.
|
||||
class Box
|
||||
{
|
||||
public:
|
||||
|
||||
/// Default ctor.
|
||||
Box() { };
|
||||
|
||||
/// Copy ctor.
|
||||
Box( const Box & b ) : m_mins(b.m_mins), m_maxs(b.m_maxs) { }
|
||||
|
||||
/// Init ctor.
|
||||
Box( Vector3::Arg mins, Vector3::Arg maxs ) : m_mins(mins), m_maxs(maxs) { }
|
||||
|
||||
// Cast operators.
|
||||
operator const float * () const { return reinterpret_cast<const float *>(this); }
|
||||
|
||||
/// Min corner of the box.
|
||||
Vector3 mins() const { return m_mins; }
|
||||
|
||||
/// Max corner of the box.
|
||||
Vector3 maxs() const { return m_maxs; }
|
||||
|
||||
/// Clear the bounds.
|
||||
void clearBounds()
|
||||
{
|
||||
m_mins.set(FLT_MAX, FLT_MAX, FLT_MAX);
|
||||
m_maxs.set(-FLT_MAX, -FLT_MAX, -FLT_MAX);
|
||||
}
|
||||
|
||||
/// Build a cube centered on center and with edge = 2*dist
|
||||
void cube(Vector3::Arg center, float dist)
|
||||
{
|
||||
setCenterExtents(center, Vector3(dist, dist, dist));
|
||||
}
|
||||
|
||||
/// Build a box, given center and extents.
|
||||
void setCenterExtents(Vector3::Arg center, Vector3::Arg extents)
|
||||
{
|
||||
m_mins = center - extents;
|
||||
m_maxs = center + extents;
|
||||
}
|
||||
|
||||
/// Get box center.
|
||||
Vector3 center() const
|
||||
{
|
||||
return (m_mins + m_maxs) * 0.5f;
|
||||
}
|
||||
|
||||
/// Return extents of the box.
|
||||
Vector3 extents() const
|
||||
{
|
||||
return (m_maxs - m_mins) * 0.5f;
|
||||
}
|
||||
|
||||
/// Add a point to this box.
|
||||
void addPointToBounds(Vector3::Arg p)
|
||||
{
|
||||
m_mins = min(m_mins, p);
|
||||
m_maxs = max(m_maxs, p);
|
||||
}
|
||||
|
||||
/// Add a box to this box.
|
||||
void addBoxToBounds(const Box & b)
|
||||
{
|
||||
m_mins = min(m_mins, b.m_mins);
|
||||
m_maxs = max(m_maxs, b.m_maxs);
|
||||
}
|
||||
|
||||
/// Translate box.
|
||||
void translate(Vector3::Arg v)
|
||||
{
|
||||
m_mins += v;
|
||||
m_maxs += v;
|
||||
}
|
||||
|
||||
/// Scale the box.
|
||||
void scale(float s)
|
||||
{
|
||||
m_mins *= s;
|
||||
m_maxs *= s;
|
||||
}
|
||||
|
||||
/// Get the area of the box.
|
||||
float area() const
|
||||
{
|
||||
const Vector3 d = extents();
|
||||
return 4.0f * (d.x()*d.y() + d.x()*d.z() + d.y()*d.z());
|
||||
}
|
||||
|
||||
/// Get the volume of the box.
|
||||
float volume() const
|
||||
{
|
||||
Vector3 d = extents();
|
||||
return 8.0f * (d.x() * d.y() * d.z());
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
Vector3 m_mins;
|
||||
Vector3 m_maxs;
|
||||
};
|
||||
|
||||
|
||||
/*
|
||||
/// Point inside box test.
|
||||
inline bool pointInsideBox(const Box & b, Vector3::Arg p) const
|
||||
{
|
||||
return (m_mins.x() < p.x() && m_mins.y() < p.y() && m_mins.z() < p.z() &&
|
||||
m_maxs.x() > p.x() && m_maxs.y() > p.y() && m_maxs.z() > p.z());
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
} // nv namespace
|
||||
|
||||
|
||||
#endif // NV_MATH_BOX_H
|
||||
24
src/nvmath/CMakeLists.txt
Normal file
24
src/nvmath/CMakeLists.txt
Normal file
@ -0,0 +1,24 @@
|
||||
PROJECT(nvmath)
|
||||
|
||||
SET(MATH_SRCS
|
||||
nvmath.h
|
||||
Vector.h
|
||||
Matrix.h
|
||||
Box.h
|
||||
Color.h
|
||||
Eigen.h Eigen.cpp
|
||||
Fitting.h Fitting.cpp)
|
||||
|
||||
INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
# targets
|
||||
ADD_DEFINITIONS(-DNVMATH_EXPORTS)
|
||||
|
||||
IF(NVMATH_SHARED)
|
||||
ADD_LIBRARY(nvmath SHARED ${MATH_SRCS})
|
||||
ELSE(NVMATH_SHARED)
|
||||
ADD_LIBRARY(nvmath ${MATH_SRCS})
|
||||
ENDIF(NVMATH_SHARED)
|
||||
|
||||
TARGET_LINK_LIBRARIES(nvmath ${LIBS} nvcore)
|
||||
|
||||
179
src/nvmath/Color.h
Normal file
179
src/nvmath/Color.h
Normal file
@ -0,0 +1,179 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_COLOR_H
|
||||
#define NV_MATH_COLOR_H
|
||||
|
||||
#include <nvcore/Debug.h>
|
||||
#include <nvmath/Vector.h>
|
||||
|
||||
namespace nv
|
||||
{
|
||||
|
||||
/// 64 bit color stored as BGRA.
|
||||
class Color64
|
||||
{
|
||||
public:
|
||||
Color64() { }
|
||||
Color64(const Color64 & c) : u(c.u) { }
|
||||
Color64(uint16 R, uint16 G, uint16 B, uint16 A) { setRGBA(R, G, B, A); }
|
||||
explicit Color64(uint64 U) : u(U) { }
|
||||
|
||||
void setRGBA(uint8 R, uint8 G, uint8 B, uint8 A)
|
||||
{
|
||||
r = R;
|
||||
g = G;
|
||||
b = B;
|
||||
a = A;
|
||||
}
|
||||
|
||||
operator uint64 () const {
|
||||
return u;
|
||||
}
|
||||
|
||||
union {
|
||||
struct {
|
||||
#if NV_LITTLE_ENDIAN
|
||||
uint16 b, g, r, a;
|
||||
#else
|
||||
uint16 a: 16;
|
||||
uint16 r: 16;
|
||||
uint16 g: 16;
|
||||
uint16 b: 16;
|
||||
#endif
|
||||
};
|
||||
uint64 u;
|
||||
};
|
||||
};
|
||||
|
||||
/// 32 bit color stored as BGRA.
|
||||
class Color32
|
||||
{
|
||||
public:
|
||||
Color32() { }
|
||||
Color32(const Color32 & c) : u(c.u) { }
|
||||
Color32(uint8 R, uint8 G, uint8 B) { setRGBA(R, G, B, 0xFF); }
|
||||
Color32(uint8 R, uint8 G, uint8 B, uint8 A) { setRGBA( R, G, B, A); }
|
||||
//Color32(uint8 c[4]) { setRGBA(c[0], c[1], c[2], c[3]); }
|
||||
//Color32(float R, float G, float B) { setRGBA(uint(R*255), uint(G*255), uint(B*255), 0xFF); }
|
||||
//Color32(float R, float G, float B, float A) { setRGBA(uint(R*255), uint(G*255), uint(B*255), uint(A*255)); }
|
||||
explicit Color32(uint32 U) : u(U) { }
|
||||
|
||||
void setRGBA(uint8 R, uint8 G, uint8 B, uint8 A)
|
||||
{
|
||||
r = R;
|
||||
g = G;
|
||||
b = B;
|
||||
a = A;
|
||||
}
|
||||
|
||||
void setBGRA(uint8 B, uint8 G, uint8 R, uint8 A = 0xFF)
|
||||
{
|
||||
r = R;
|
||||
g = G;
|
||||
b = B;
|
||||
a = A;
|
||||
}
|
||||
|
||||
operator uint32 () const {
|
||||
return u;
|
||||
}
|
||||
|
||||
union {
|
||||
struct {
|
||||
#if NV_LITTLE_ENDIAN
|
||||
uint8 b, g, r, a;
|
||||
#else
|
||||
uint8 a: 8;
|
||||
uint8 r: 8;
|
||||
uint8 g: 8;
|
||||
uint8 b: 8;
|
||||
#endif
|
||||
};
|
||||
uint32 u;
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
/// 16 bit 565 BGR color.
|
||||
class Color16
|
||||
{
|
||||
public:
|
||||
Color16() { }
|
||||
Color16(const Color16 & c) : u(c.u) { }
|
||||
explicit Color16(uint16 U) : u(U) { }
|
||||
|
||||
union {
|
||||
struct {
|
||||
#if NV_LITTLE_ENDIAN
|
||||
uint16 b : 5;
|
||||
uint16 g : 6;
|
||||
uint16 r : 5;
|
||||
#else
|
||||
uint16 r : 5;
|
||||
uint16 g : 6;
|
||||
uint16 b : 5;
|
||||
#endif
|
||||
};
|
||||
uint16 u;
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
/// Clamp color components.
|
||||
inline Vector3 colorClamp(Vector3::Arg c)
|
||||
{
|
||||
return Vector3(clamp(c.x(), 0.0f, 1.0f), clamp(c.y(), 0.0f, 1.0f), clamp(c.z(), 0.0f, 1.0f));
|
||||
}
|
||||
|
||||
/// Clamp without allowing the hue to change.
|
||||
inline Vector3 colorNormalize(Vector3::Arg c)
|
||||
{
|
||||
float scale = 1.0f;
|
||||
if (c.x() > scale) scale = c.x();
|
||||
if (c.y() > scale) scale = c.y();
|
||||
if (c.z() > scale) scale = c.z();
|
||||
return c / scale;
|
||||
}
|
||||
|
||||
/// Convert Color32 to Color16.
|
||||
inline Color16 toColor16(Color32 c)
|
||||
{
|
||||
Color16 color;
|
||||
// rrrrrggggggbbbbb
|
||||
// rrrrr000gggggg00bbbbb000
|
||||
// color.u = (c.u >> 3) & 0x1F;
|
||||
// color.u |= (c.u >> 5) & 0x7E0;
|
||||
// color.u |= (c.u >> 8) & 0xF800;
|
||||
|
||||
color.r = c.r >> 3;
|
||||
color.g = c.g >> 2;
|
||||
color.b = c.b >> 3;
|
||||
return color;
|
||||
}
|
||||
|
||||
|
||||
/// Promote 16 bit color to 32 bit using regular bit expansion.
|
||||
inline Color32 toColor32(Color16 c)
|
||||
{
|
||||
Color32 color;
|
||||
// c.u = ((col0.u << 3) & 0xf8) | ((col0.u << 5) & 0xfc00) | ((col0.u << 8) & 0xf80000);
|
||||
// c.u |= (c.u >> 5) & 0x070007;
|
||||
// c.u |= (c.u >> 6) & 0x000300;
|
||||
|
||||
color.b = (c.b << 3) | (c.b >> 2);
|
||||
color.g = (c.g << 2) | (c.g >> 4);
|
||||
color.r = (c.r << 3) | (c.r >> 2);
|
||||
color.a = 0xFF;
|
||||
|
||||
return color;
|
||||
}
|
||||
|
||||
inline Vector4 toVector4(Color32 c)
|
||||
{
|
||||
const float scale = 1.0f / 255.0f;
|
||||
return Vector4(c.r * scale, c.g * scale, c.b * scale, c.a * scale);
|
||||
}
|
||||
|
||||
} // nv namespace
|
||||
|
||||
#endif // NV_MATH_COLOR_H
|
||||
533
src/nvmath/Eigen.cpp
Normal file
533
src/nvmath/Eigen.cpp
Normal file
@ -0,0 +1,533 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#include "Eigen.h"
|
||||
|
||||
using namespace nv;
|
||||
|
||||
static const float EPS = 0.00001f;
|
||||
static const int MAX_ITER = 100;
|
||||
|
||||
static void semi_definite_symmetric_eigen(const float *mat, int n, float *eigen_vec, float *eigen_val);
|
||||
|
||||
|
||||
// Use power method to find the first eigenvector.
|
||||
// http://www.miislita.com/information-retrieval-tutorial/matrix-tutorial-3-eigenvalues-eigenvectors.html
|
||||
Vector3 nv::firstEigenVector(float matrix[6])
|
||||
{
|
||||
// Number of iterations. @@ Use a variable number of iterations.
|
||||
const int NUM = 8;
|
||||
|
||||
Vector3 v(1, 1, 1);
|
||||
for(int i = 0; i < NUM; i++) {
|
||||
float x = v.x() * matrix[0] + v.y() * matrix[1] + v.z() * matrix[2];
|
||||
float y = v.x() * matrix[1] + v.y() * matrix[3] + v.z() * matrix[4];
|
||||
float z = v.x() * matrix[2] + v.y() * matrix[4] + v.z() * matrix[5];
|
||||
|
||||
float norm = max(max(x, y), z);
|
||||
float iv = 1.0f / norm;
|
||||
if (norm == 0.0f) {
|
||||
return Vector3(zero);
|
||||
}
|
||||
|
||||
v.set(x*iv, y*iv, z*iv);
|
||||
}
|
||||
|
||||
return v;
|
||||
}
|
||||
|
||||
|
||||
/// Solve eigen system.
|
||||
void Eigen::solve() {
|
||||
semi_definite_symmetric_eigen(matrix, N, eigen_vec, eigen_val);
|
||||
}
|
||||
|
||||
/// Solve eigen system.
|
||||
void Eigen3::solve() {
|
||||
// @@ Use lengyel code that seems to be more optimized.
|
||||
#if 1
|
||||
float v[3*3];
|
||||
semi_definite_symmetric_eigen(matrix, 3, v, eigen_val);
|
||||
|
||||
eigen_vec[0].set(v[0], v[1], v[2]);
|
||||
eigen_vec[1].set(v[3], v[4], v[5]);
|
||||
eigen_vec[2].set(v[6], v[7], v[8]);
|
||||
#else
|
||||
const int maxSweeps = 32;
|
||||
const float epsilon = 1.0e-10f;
|
||||
|
||||
float m11 = matrix[0]; // m(0,0);
|
||||
float m12 = matrix[1]; // m(0,1);
|
||||
float m13 = matrix[2]; // m(0,2);
|
||||
float m22 = matrix[3]; // m(1,1);
|
||||
float m23 = matrix[4]; // m(1,2);
|
||||
float m33 = matrix[5]; // m(2,2);
|
||||
|
||||
//r.SetIdentity();
|
||||
eigen_vec[0].set(1, 0, 0);
|
||||
eigen_vec[1].set(0, 1, 0);
|
||||
eigen_vec[2].set(0, 0, 1);
|
||||
|
||||
for (int a = 0; a < maxSweeps; a++)
|
||||
{
|
||||
// Exit if off-diagonal entries small enough
|
||||
if ((fabs(m12) < epsilon) && (fabs(m13) < epsilon) && (fabs(m23) < epsilon))
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
// Annihilate (1,2) entry
|
||||
if (m12 != 0.0f)
|
||||
{
|
||||
float u = (m22 - m11) * 0.5f / m12;
|
||||
float u2 = u * u;
|
||||
float u2p1 = u2 + 1.0f;
|
||||
float t = (u2p1 != u2) ? ((u < 0.0f) ? -1.0f : 1.0f) * (sqrt(u2p1) - fabs(u)) : 0.5f / u;
|
||||
float c = 1.0f / sqrt(t * t + 1.0f);
|
||||
float s = c * t;
|
||||
|
||||
m11 -= t * m12;
|
||||
m22 += t * m12;
|
||||
m12 = 0.0f;
|
||||
|
||||
float temp = c * m13 - s * m23;
|
||||
m23 = s * m13 + c * m23;
|
||||
m13 = temp;
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
float temp = c * eigen_vec[i].x - s * eigen_vec[i].y;
|
||||
eigen_vec[i].y = s * eigen_vec[i].x + c * eigen_vec[i].y;
|
||||
eigen_vec[i].x = temp;
|
||||
}
|
||||
}
|
||||
|
||||
// Annihilate (1,3) entry
|
||||
if (m13 != 0.0f)
|
||||
{
|
||||
float u = (m33 - m11) * 0.5f / m13;
|
||||
float u2 = u * u;
|
||||
float u2p1 = u2 + 1.0f;
|
||||
float t = (u2p1 != u2) ? ((u < 0.0f) ? -1.0f : 1.0f) * (sqrt(u2p1) - fabs(u)) : 0.5f / u;
|
||||
float c = 1.0f / sqrt(t * t + 1.0f);
|
||||
float s = c * t;
|
||||
|
||||
m11 -= t * m13;
|
||||
m33 += t * m13;
|
||||
m13 = 0.0f;
|
||||
|
||||
float temp = c * m12 - s * m23;
|
||||
m23 = s * m12 + c * m23;
|
||||
m12 = temp;
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
float temp = c * eigen_vec[i].x - s * eigen_vec[i].z;
|
||||
eigen_vec[i].z = s * eigen_vec[i].x + c * eigen_vec[i].z;
|
||||
eigen_vec[i].x = temp;
|
||||
}
|
||||
}
|
||||
|
||||
// Annihilate (2,3) entry
|
||||
if (m23 != 0.0f)
|
||||
{
|
||||
float u = (m33 - m22) * 0.5f / m23;
|
||||
float u2 = u * u;
|
||||
float u2p1 = u2 + 1.0f;
|
||||
float t = (u2p1 != u2) ? ((u < 0.0f) ? -1.0f : 1.0f) * (sqrt(u2p1) - fabs(u)) : 0.5f / u;
|
||||
float c = 1.0f / sqrt(t * t + 1.0f);
|
||||
float s = c * t;
|
||||
|
||||
m22 -= t * m23;
|
||||
m33 += t * m23;
|
||||
m23 = 0.0f;
|
||||
|
||||
float temp = c * m12 - s * m13;
|
||||
m13 = s * m12 + c * m13;
|
||||
m12 = temp;
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
float temp = c * eigen_vec[i].y - s * eigen_vec[i].z;
|
||||
eigen_vec[i].z = s * eigen_vec[i].y + c * eigen_vec[i].z;
|
||||
eigen_vec[i].y = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
eigen_val[0] = m11;
|
||||
eigen_val[1] = m22;
|
||||
eigen_val[2] = m33;
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
/*---------------------------------------------------------------------------
|
||||
Functions
|
||||
---------------------------------------------------------------------------*/
|
||||
|
||||
|
||||
/** @@ I don't remember where did I get this function.
|
||||
* computes the eigen values and eigen vectors
|
||||
* of a semi definite symmetric matrix
|
||||
*
|
||||
* - matrix is stored in column symmetric storage, i.e.
|
||||
* matrix = { m11, m12, m22, m13, m23, m33, m14, m24, m34, m44 ... }
|
||||
* size = n(n+1)/2
|
||||
*
|
||||
* - eigen_vectors (return) = { v1, v2, v3, ..., vn } where vk = vk0, vk1, ..., vkn
|
||||
* size = n^2, must be allocated by caller
|
||||
*
|
||||
* - eigen_values (return) are in decreasing order
|
||||
* size = n, must be allocated by caller
|
||||
*/
|
||||
|
||||
void semi_definite_symmetric_eigen(
|
||||
const float *mat, int n, float *eigen_vec, float *eigen_val
|
||||
) {
|
||||
float *a,*v;
|
||||
float a_norm,a_normEPS,thr,thr_nn;
|
||||
int nb_iter = 0;
|
||||
int jj;
|
||||
int i,j,k,ij,ik,l,m,lm,mq,lq,ll,mm,imv,im,iq,ilv,il,nn;
|
||||
int *index;
|
||||
float a_ij,a_lm,a_ll,a_mm,a_im,a_il;
|
||||
float a_lm_2;
|
||||
float v_ilv,v_imv;
|
||||
float x;
|
||||
float sinx,sinx_2,cosx,cosx_2,sincos;
|
||||
float delta;
|
||||
|
||||
// Number of entries in mat
|
||||
|
||||
nn = (n*(n+1))/2;
|
||||
|
||||
// Step 1: Copy mat to a
|
||||
|
||||
a = new float[nn];
|
||||
|
||||
for( ij=0; ij<nn; ij++ ) {
|
||||
a[ij] = mat[ij];
|
||||
}
|
||||
|
||||
// Ugly Fortran-porting trick: indices for a are between 1 and n
|
||||
a--;
|
||||
|
||||
// Step 2 : Init diagonalization matrix as the unit matrix
|
||||
v = new float[n*n];
|
||||
|
||||
ij = 0;
|
||||
for( i=0; i<n; i++ ) {
|
||||
for( j=0; j<n; j++ ) {
|
||||
if( i==j ) {
|
||||
v[ij++] = 1.0;
|
||||
} else {
|
||||
v[ij++] = 0.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Ugly Fortran-porting trick: indices for v are between 1 and n
|
||||
v--;
|
||||
|
||||
// Step 3 : compute the weight of the non diagonal terms
|
||||
ij = 1 ;
|
||||
a_norm = 0.0;
|
||||
for( i=1; i<=n; i++ ) {
|
||||
for( j=1; j<=i; j++ ) {
|
||||
if( i!=j ) {
|
||||
a_ij = a[ij];
|
||||
a_norm += a_ij*a_ij;
|
||||
}
|
||||
ij++;
|
||||
}
|
||||
}
|
||||
|
||||
if( a_norm != 0.0 ) {
|
||||
|
||||
a_normEPS = a_norm*EPS;
|
||||
thr = a_norm ;
|
||||
|
||||
// Step 4 : rotations
|
||||
while( thr > a_normEPS && nb_iter < MAX_ITER ) {
|
||||
|
||||
nb_iter++;
|
||||
thr_nn = thr / nn;
|
||||
|
||||
for( l=1 ; l< n; l++ ) {
|
||||
for( m=l+1; m<=n; m++ ) {
|
||||
|
||||
// compute sinx and cosx
|
||||
|
||||
lq = (l*l-l)/2;
|
||||
mq = (m*m-m)/2;
|
||||
|
||||
lm = l+mq;
|
||||
a_lm = a[lm];
|
||||
a_lm_2 = a_lm*a_lm;
|
||||
|
||||
if( a_lm_2 < thr_nn ) {
|
||||
continue ;
|
||||
}
|
||||
|
||||
ll = l+lq;
|
||||
mm = m+mq;
|
||||
a_ll = a[ll];
|
||||
a_mm = a[mm];
|
||||
|
||||
delta = a_ll - a_mm;
|
||||
|
||||
if( delta == 0.0 ) {
|
||||
x = - PI/4 ;
|
||||
} else {
|
||||
x = - atan( (a_lm+a_lm) / delta ) / 2.0 ;
|
||||
}
|
||||
|
||||
sinx = sin(x) ;
|
||||
cosx = cos(x) ;
|
||||
sinx_2 = sinx*sinx;
|
||||
cosx_2 = cosx*cosx;
|
||||
sincos = sinx*cosx;
|
||||
|
||||
// rotate L and M columns
|
||||
|
||||
ilv = n*(l-1);
|
||||
imv = n*(m-1);
|
||||
|
||||
for( i=1; i<=n;i++ ) {
|
||||
if( (i!=l) && (i!=m) ) {
|
||||
iq = (i*i-i)/2;
|
||||
|
||||
if( i<m ) {
|
||||
im = i + mq;
|
||||
} else {
|
||||
im = m + iq;
|
||||
}
|
||||
a_im = a[im];
|
||||
|
||||
if( i<l ) {
|
||||
il = i + lq;
|
||||
} else {
|
||||
il = l + iq;
|
||||
}
|
||||
a_il = a[il];
|
||||
|
||||
a[il] = a_il*cosx - a_im*sinx;
|
||||
a[im] = a_il*sinx + a_im*cosx;
|
||||
}
|
||||
|
||||
ilv++;
|
||||
imv++;
|
||||
|
||||
v_ilv = v[ilv];
|
||||
v_imv = v[imv];
|
||||
|
||||
v[ilv] = cosx*v_ilv - sinx*v_imv;
|
||||
v[imv] = sinx*v_ilv + cosx*v_imv;
|
||||
}
|
||||
|
||||
x = a_lm*sincos; x+=x;
|
||||
|
||||
a[ll] = a_ll*cosx_2 + a_mm*sinx_2 - x;
|
||||
a[mm] = a_ll*sinx_2 + a_mm*cosx_2 + x;
|
||||
a[lm] = 0.0;
|
||||
|
||||
thr = fabs( thr - a_lm_2 );
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Step 5: index conversion and copy eigen values
|
||||
|
||||
// back from Fortran to C++
|
||||
a++;
|
||||
|
||||
for( i=0; i<n; i++ ) {
|
||||
k = i + (i*(i+1))/2;
|
||||
eigen_val[i] = a[k];
|
||||
}
|
||||
|
||||
delete[] a;
|
||||
|
||||
// Step 6: sort the eigen values and eigen vectors
|
||||
|
||||
index = new int[n];
|
||||
for( i=0; i<n; i++ ) {
|
||||
index[i] = i;
|
||||
}
|
||||
|
||||
for( i=0; i<(n-1); i++ ) {
|
||||
x = eigen_val[i];
|
||||
k = i;
|
||||
|
||||
for( j=i+1; j<n; j++ ) {
|
||||
if( x < eigen_val[j] ) {
|
||||
k = j;
|
||||
x = eigen_val[j];
|
||||
}
|
||||
}
|
||||
|
||||
eigen_val[k] = eigen_val[i];
|
||||
eigen_val[i] = x;
|
||||
|
||||
jj = index[k];
|
||||
index[k] = index[i];
|
||||
index[i] = jj;
|
||||
}
|
||||
|
||||
|
||||
// Step 7: save the eigen vectors
|
||||
|
||||
v++; // back from Fortran to to C++
|
||||
|
||||
ij = 0;
|
||||
for( k=0; k<n; k++ ) {
|
||||
ik = index[k]*n;
|
||||
for( i=0; i<n; i++ ) {
|
||||
eigen_vec[ij++] = v[ik++];
|
||||
}
|
||||
}
|
||||
|
||||
delete[] v ;
|
||||
delete[] index;
|
||||
return;
|
||||
}
|
||||
|
||||
//_________________________________________________________
|
||||
|
||||
|
||||
// Eric Lengyel code:
|
||||
// http://www.terathon.com/code/linear.html
|
||||
#if 0
|
||||
|
||||
const float epsilon = 1.0e-10F;
|
||||
const int maxSweeps = 32;
|
||||
|
||||
|
||||
struct Matrix3D
|
||||
{
|
||||
float n[3][3];
|
||||
|
||||
float& operator()(int i, int j)
|
||||
{
|
||||
return (n[j][i]);
|
||||
}
|
||||
|
||||
const float& operator()(int i, int j) const
|
||||
{
|
||||
return (n[j][i]);
|
||||
}
|
||||
|
||||
void SetIdentity(void)
|
||||
{
|
||||
n[0][0] = n[1][1] = n[2][2] = 1.0F;
|
||||
n[0][1] = n[0][2] = n[1][0] = n[1][2] = n[2][0] = n[2][1] = 0.0F;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
void CalculateEigensystem(const Matrix3D& m, float *lambda, Matrix3D& r)
|
||||
{
|
||||
float m11 = m(0,0);
|
||||
float m12 = m(0,1);
|
||||
float m13 = m(0,2);
|
||||
float m22 = m(1,1);
|
||||
float m23 = m(1,2);
|
||||
float m33 = m(2,2);
|
||||
|
||||
r.SetIdentity();
|
||||
for (int a = 0; a < maxSweeps; a++)
|
||||
{
|
||||
// Exit if off-diagonal entries small enough
|
||||
if ((Fabs(m12) < epsilon) && (Fabs(m13) < epsilon) &&
|
||||
(Fabs(m23) < epsilon)) break;
|
||||
|
||||
// Annihilate (1,2) entry
|
||||
if (m12 != 0.0F)
|
||||
{
|
||||
float u = (m22 - m11) * 0.5F / m12;
|
||||
float u2 = u * u;
|
||||
float u2p1 = u2 + 1.0F;
|
||||
float t = (u2p1 != u2) ?
|
||||
((u < 0.0F) ? -1.0F : 1.0F) * (sqrt(u2p1) - fabs(u)) : 0.5F / u;
|
||||
float c = 1.0F / sqrt(t * t + 1.0F);
|
||||
float s = c * t;
|
||||
|
||||
m11 -= t * m12;
|
||||
m22 += t * m12;
|
||||
m12 = 0.0F;
|
||||
|
||||
float temp = c * m13 - s * m23;
|
||||
m23 = s * m13 + c * m23;
|
||||
m13 = temp;
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
float temp = c * r(i,0) - s * r(i,1);
|
||||
r(i,1) = s * r(i,0) + c * r(i,1);
|
||||
r(i,0) = temp;
|
||||
}
|
||||
}
|
||||
|
||||
// Annihilate (1,3) entry
|
||||
if (m13 != 0.0F)
|
||||
{
|
||||
float u = (m33 - m11) * 0.5F / m13;
|
||||
float u2 = u * u;
|
||||
float u2p1 = u2 + 1.0F;
|
||||
float t = (u2p1 != u2) ?
|
||||
((u < 0.0F) ? -1.0F : 1.0F) * (sqrt(u2p1) - fabs(u)) : 0.5F / u;
|
||||
float c = 1.0F / sqrt(t * t + 1.0F);
|
||||
float s = c * t;
|
||||
|
||||
m11 -= t * m13;
|
||||
m33 += t * m13;
|
||||
m13 = 0.0F;
|
||||
|
||||
float temp = c * m12 - s * m23;
|
||||
m23 = s * m12 + c * m23;
|
||||
m12 = temp;
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
float temp = c * r(i,0) - s * r(i,2);
|
||||
r(i,2) = s * r(i,0) + c * r(i,2);
|
||||
r(i,0) = temp;
|
||||
}
|
||||
}
|
||||
|
||||
// Annihilate (2,3) entry
|
||||
if (m23 != 0.0F)
|
||||
{
|
||||
float u = (m33 - m22) * 0.5F / m23;
|
||||
float u2 = u * u;
|
||||
float u2p1 = u2 + 1.0F;
|
||||
float t = (u2p1 != u2) ?
|
||||
((u < 0.0F) ? -1.0F : 1.0F) * (sqrt(u2p1) - fabs(u)) : 0.5F / u;
|
||||
float c = 1.0F / sqrt(t * t + 1.0F);
|
||||
float s = c * t;
|
||||
|
||||
m22 -= t * m23;
|
||||
m33 += t * m23;
|
||||
m23 = 0.0F;
|
||||
|
||||
float temp = c * m12 - s * m13;
|
||||
m13 = s * m12 + c * m13;
|
||||
m12 = temp;
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
float temp = c * r(i,1) - s * r(i,2);
|
||||
r(i,2) = s * r(i,1) + c * r(i,2);
|
||||
r(i,1) = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
lambda[0] = m11;
|
||||
lambda[1] = m22;
|
||||
lambda[2] = m33;
|
||||
}
|
||||
|
||||
|
||||
#endif
|
||||
140
src/nvmath/Eigen.h
Normal file
140
src/nvmath/Eigen.h
Normal file
@ -0,0 +1,140 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_EIGEN_H
|
||||
#define NV_MATH_EIGEN_H
|
||||
|
||||
#include <nvcore/Containers.h> // swap
|
||||
#include <nvmath/nvmath.h>
|
||||
#include <nvmath/Vector.h>
|
||||
|
||||
namespace nv
|
||||
{
|
||||
|
||||
// Compute first eigen vector using the power method.
|
||||
Vector3 firstEigenVector(float matrix[6]);
|
||||
|
||||
/// Generic eigen-solver.
|
||||
class Eigen
|
||||
{
|
||||
public:
|
||||
|
||||
/// Ctor.
|
||||
Eigen(uint n) : N(n)
|
||||
{
|
||||
uint size = n * (n + 1) / 2;
|
||||
matrix = new float[size];
|
||||
eigen_vec = new float[N*N];
|
||||
eigen_val = new float[N];
|
||||
}
|
||||
|
||||
/// Dtor.
|
||||
~Eigen()
|
||||
{
|
||||
delete [] matrix;
|
||||
delete [] eigen_vec;
|
||||
delete [] eigen_val;
|
||||
}
|
||||
|
||||
NVMATH_API void solve();
|
||||
|
||||
/// Matrix accesor.
|
||||
float & operator()(uint x, uint y)
|
||||
{
|
||||
if( x > y ) {
|
||||
swap(x, y);
|
||||
}
|
||||
return matrix[y * (y + 1) / 2 + x];
|
||||
}
|
||||
|
||||
/// Matrix const accessor.
|
||||
float operator()(uint x, uint y) const
|
||||
{
|
||||
if( x > y ) {
|
||||
swap(x, y);
|
||||
}
|
||||
return matrix[y * (y + 1) / 2 + x];
|
||||
}
|
||||
|
||||
Vector3 eigenVector3(uint i) const
|
||||
{
|
||||
nvCheck(3 == N);
|
||||
nvCheck(i < N);
|
||||
return Vector3(eigen_vec[i*N+0], eigen_vec[i*N+1], eigen_vec[i*N+2]);
|
||||
}
|
||||
|
||||
Vector4 eigenVector4(uint i) const
|
||||
{
|
||||
nvCheck(4 == N);
|
||||
nvCheck(i < N);
|
||||
return Vector4(eigen_vec[i*N+0], eigen_vec[i*N+1], eigen_vec[i*N+2], eigen_vec[i*N+3]);
|
||||
}
|
||||
|
||||
float eigenValue(uint i) const
|
||||
{
|
||||
nvCheck(i < N);
|
||||
return eigen_val[i];
|
||||
}
|
||||
|
||||
private:
|
||||
const uint N;
|
||||
float * matrix;
|
||||
float * eigen_vec;
|
||||
float * eigen_val;
|
||||
};
|
||||
|
||||
|
||||
/// 3x3 eigen-solver.
|
||||
/// Based on Eric Lengyel's code:
|
||||
/// http://www.terathon.com/code/linear.html
|
||||
class Eigen3
|
||||
{
|
||||
public:
|
||||
|
||||
/** Ctor. */
|
||||
Eigen3() {}
|
||||
|
||||
NVMATH_API void solve();
|
||||
|
||||
/// Matrix accesor.
|
||||
float & operator()(uint x, uint y)
|
||||
{
|
||||
nvDebugCheck( x < 3 && y < 3 );
|
||||
if( x > y ) {
|
||||
swap(x, y);
|
||||
}
|
||||
return matrix[y * (y + 1) / 2 + x];
|
||||
}
|
||||
|
||||
/// Matrix const accessor.
|
||||
float operator()(uint x, uint y) const
|
||||
{
|
||||
nvDebugCheck( x < 3 && y < 3 );
|
||||
if( x > y ) {
|
||||
swap(x, y);
|
||||
}
|
||||
return matrix[y * (y + 1) / 2 + x];
|
||||
}
|
||||
|
||||
/// Get ith eigen vector.
|
||||
Vector3 eigenVector(uint i) const
|
||||
{
|
||||
nvCheck(i < 3);
|
||||
return eigen_vec[i];
|
||||
}
|
||||
|
||||
/** Get ith eigen value. */
|
||||
float eigenValue(uint i) const
|
||||
{
|
||||
nvCheck(i < 3);
|
||||
return eigen_val[i];
|
||||
}
|
||||
|
||||
private:
|
||||
float matrix[3+2+1];
|
||||
Vector3 eigen_vec[3];
|
||||
float eigen_val[3];
|
||||
};
|
||||
|
||||
} // nv namespace
|
||||
|
||||
#endif // NV_MATH_EIGEN_H
|
||||
134
src/nvmath/Fitting.cpp
Normal file
134
src/nvmath/Fitting.cpp
Normal file
@ -0,0 +1,134 @@
|
||||
// License: Wild Magic License Version 3
|
||||
// http://geometrictools.com/License/WildMagic3License.pdf
|
||||
|
||||
#include "Fitting.h"
|
||||
#include "Eigen.h"
|
||||
|
||||
using namespace nv;
|
||||
|
||||
|
||||
/** Fit a 3d line to the given set of points.
|
||||
*
|
||||
* Based on code from:
|
||||
* http://geometrictools.com/
|
||||
*/
|
||||
Line3 Fit::bestLine(const Array<Vector3> & pointArray)
|
||||
{
|
||||
nvDebugCheck(pointArray.count() > 0);
|
||||
|
||||
Line3 line;
|
||||
|
||||
const uint pointCount = pointArray.count();
|
||||
const float inv_num = 1.0f / pointCount;
|
||||
|
||||
// compute the mean of the points
|
||||
Vector3 center(zero);
|
||||
for(uint i = 0; i < pointCount; i++) {
|
||||
center += pointArray[i];
|
||||
}
|
||||
line.setOrigin(center * inv_num);
|
||||
|
||||
// compute the covariance matrix of the points
|
||||
float covariance[6] = {0, 0, 0, 0, 0, 0};
|
||||
for(uint i = 0; i < pointCount; i++) {
|
||||
Vector3 diff = pointArray[i] - line.origin();
|
||||
covariance[0] += diff.x() * diff.x();
|
||||
covariance[1] += diff.x() * diff.y();
|
||||
covariance[2] += diff.x() * diff.z();
|
||||
covariance[3] += diff.y() * diff.y();
|
||||
covariance[4] += diff.y() * diff.z();
|
||||
covariance[5] += diff.z() * diff.z();
|
||||
}
|
||||
|
||||
line.setDirection(normalizeSafe(firstEigenVector(covariance), Vector3(zero), 0.0f));
|
||||
|
||||
// @@ This variant is from David Eberly... I'm not sure how that works.
|
||||
/*sum_xx *= inv_num;
|
||||
sum_xy *= inv_num;
|
||||
sum_xz *= inv_num;
|
||||
sum_yy *= inv_num;
|
||||
sum_yz *= inv_num;
|
||||
sum_zz *= inv_num;
|
||||
|
||||
// set up the eigensolver
|
||||
Eigen3 ES;
|
||||
ES(0,0) = sum_yy + sum_zz;
|
||||
ES(0,1) = -sum_xy;
|
||||
ES(0,2) = -sum_xz;
|
||||
ES(1,1) = sum_xx + sum_zz;
|
||||
ES(1,2) = -sum_yz;
|
||||
ES(2,2) = sum_xx + sum_yy;
|
||||
|
||||
// compute eigenstuff, smallest eigenvalue is in last position
|
||||
ES.solve();
|
||||
|
||||
line.setDirection(ES.eigenVector(2));
|
||||
|
||||
nvCheck( isNormalized(line.direction()) );
|
||||
*/
|
||||
return line;
|
||||
}
|
||||
|
||||
|
||||
/** Fit a 3d plane to the given set of points.
|
||||
*
|
||||
* Based on code from:
|
||||
* http://geometrictools.com/
|
||||
*/
|
||||
Vector4 Fit::bestPlane(const Array<Vector3> & pointArray)
|
||||
{
|
||||
Vector3 center(zero);
|
||||
|
||||
const uint pointCount = pointArray.count();
|
||||
const float inv_num = 1.0f / pointCount;
|
||||
|
||||
// compute the mean of the points
|
||||
for(uint i = 0; i < pointCount; i++) {
|
||||
center += pointArray[i];
|
||||
}
|
||||
center *= inv_num;
|
||||
|
||||
// compute the covariance matrix of the points
|
||||
float sum_xx = 0.0f;
|
||||
float sum_xy = 0.0f;
|
||||
float sum_xz = 0.0f;
|
||||
float sum_yy = 0.0f;
|
||||
float sum_yz = 0.0f;
|
||||
float sum_zz = 0.0f;
|
||||
|
||||
for(uint i = 0; i < pointCount; i++) {
|
||||
Vector3 diff = pointArray[i] - center;
|
||||
sum_xx += diff.x() * diff.x();
|
||||
sum_xy += diff.x() * diff.y();
|
||||
sum_xz += diff.x() * diff.z();
|
||||
sum_yy += diff.y() * diff.y();
|
||||
sum_yz += diff.y() * diff.z();
|
||||
sum_zz += diff.z() * diff.z();
|
||||
}
|
||||
|
||||
sum_xx *= inv_num;
|
||||
sum_xy *= inv_num;
|
||||
sum_xz *= inv_num;
|
||||
sum_yy *= inv_num;
|
||||
sum_yz *= inv_num;
|
||||
sum_zz *= inv_num;
|
||||
|
||||
// set up the eigensolver
|
||||
Eigen3 ES;
|
||||
ES(0,0) = sum_yy + sum_zz;
|
||||
ES(0,1) = -sum_xy;
|
||||
ES(0,2) = -sum_xz;
|
||||
ES(1,1) = sum_xx + sum_zz;
|
||||
ES(1,2) = -sum_yz;
|
||||
ES(2,2) = sum_xx + sum_yy;
|
||||
|
||||
// compute eigenstuff, greatest eigenvalue is in first position
|
||||
ES.solve();
|
||||
|
||||
Vector3 normal = ES.eigenVector(0);
|
||||
nvCheck(isNormalized(normal));
|
||||
|
||||
float offset = dot(normal, center);
|
||||
|
||||
return Vector4(normal, offset);
|
||||
}
|
||||
78
src/nvmath/Fitting.h
Normal file
78
src/nvmath/Fitting.h
Normal file
@ -0,0 +1,78 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_FITTING_H
|
||||
#define NV_MATH_FITTING_H
|
||||
|
||||
#include <nvmath/Vector.h>
|
||||
|
||||
namespace nv
|
||||
{
|
||||
|
||||
/// 3D Line.
|
||||
struct Line3
|
||||
{
|
||||
/// Ctor.
|
||||
Line3() : m_origin(zero), m_direction(zero)
|
||||
{
|
||||
}
|
||||
|
||||
/// Copy ctor.
|
||||
Line3(const Line3 & l) : m_origin(l.m_origin), m_direction(l.m_direction)
|
||||
{
|
||||
}
|
||||
|
||||
/// Ctor.
|
||||
Line3(Vector3::Arg o, Vector3::Arg d) : m_origin(o), m_direction(d)
|
||||
{
|
||||
}
|
||||
|
||||
/// Normalize the line.
|
||||
void normalize()
|
||||
{
|
||||
m_direction = nv::normalize(m_direction);
|
||||
}
|
||||
|
||||
/// Project a point onto the line.
|
||||
Vector3 projectPoint(Vector3::Arg point) const
|
||||
{
|
||||
nvDebugCheck(isNormalized(m_direction));
|
||||
|
||||
Vector3 v = point - m_origin;
|
||||
return m_origin + m_direction * dot(m_direction, v);
|
||||
}
|
||||
|
||||
/// Compute distance to line.
|
||||
float distanceToPoint(Vector3::Arg point) const
|
||||
{
|
||||
nvDebugCheck(isNormalized(m_direction));
|
||||
|
||||
Vector3 v = point - m_origin;
|
||||
Vector3 l = v - m_direction * dot(m_direction, v);
|
||||
|
||||
return length(l);
|
||||
}
|
||||
|
||||
const Vector3 & origin() const { return m_origin; }
|
||||
void setOrigin(Vector3::Arg value) { m_origin = value; }
|
||||
|
||||
const Vector3 & direction() const { return m_direction; }
|
||||
void setDirection(Vector3::Arg value) { m_direction = value; }
|
||||
|
||||
|
||||
private:
|
||||
Vector3 m_origin;
|
||||
Vector3 m_direction;
|
||||
};
|
||||
|
||||
|
||||
namespace Fit
|
||||
{
|
||||
|
||||
NVMATH_API Line3 bestLine(const Array<Vector3> & pointArray);
|
||||
NVMATH_API Vector4 bestPlane(const Array<Vector3> & pointArray);
|
||||
|
||||
} // Fit namespace
|
||||
|
||||
} // nv namespace
|
||||
|
||||
#endif // _PI_MATHLIB_FITTING_H_
|
||||
933
src/nvmath/Matrix.h
Normal file
933
src/nvmath/Matrix.h
Normal file
@ -0,0 +1,933 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_MATRIX_H
|
||||
#define NV_MATH_MATRIX_H
|
||||
|
||||
#include <nvmath/nvmath.h>
|
||||
#include <nvmath/Vector.h>
|
||||
|
||||
namespace nv
|
||||
{
|
||||
|
||||
// @@ Use scalar defined in Vector.h, but should use a template instead.
|
||||
|
||||
/// 4x4 transformation matrix.
|
||||
/// -# Matrices are stored in memory in column major order.
|
||||
/// -# Points are to be though of as column vectors.
|
||||
/// -# Transformation of a point p by a matrix M is: p' = M * p
|
||||
class NVMATH_CLASS Matrix
|
||||
{
|
||||
public:
|
||||
typedef Matrix const & Arg;
|
||||
|
||||
Matrix();
|
||||
Matrix(zero_t);
|
||||
Matrix(identity_t);
|
||||
Matrix(const Matrix & m);
|
||||
|
||||
scalar get(uint row, uint col) const;
|
||||
scalar operator()(uint row, uint col) const;
|
||||
scalar & operator()(uint row, uint col);
|
||||
const scalar * ptr() const;
|
||||
|
||||
Vector4 row(uint i) const;
|
||||
Vector4 column(uint i) const;
|
||||
|
||||
|
||||
void scale(scalar s);
|
||||
void scale(Vector3::Arg s);
|
||||
void translate(Vector3::Arg t);
|
||||
void rotate(scalar theta, scalar v0, scalar v1, scalar v2);
|
||||
|
||||
void apply(Matrix::Arg m);
|
||||
|
||||
private:
|
||||
scalar m_data[16];
|
||||
};
|
||||
|
||||
|
||||
inline Matrix::Matrix()
|
||||
{
|
||||
}
|
||||
|
||||
inline Matrix::Matrix(zero_t)
|
||||
{
|
||||
for(int i = 0; i < 16; i++) {
|
||||
m_data[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
inline Matrix::Matrix(identity_t)
|
||||
{
|
||||
for(int i = 0; i < 4; i++) {
|
||||
for(int j = 0; j < 4; j++) {
|
||||
m_data[4*j+i] = (i == j) ? 1.0f : 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline Matrix::Matrix(const Matrix & m)
|
||||
{
|
||||
for(int i = 0; i < 16; i++) {
|
||||
m_data[i] = m.m_data[i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Accessors
|
||||
inline scalar Matrix::get(uint row, uint col) const
|
||||
{
|
||||
nvDebugCheck(row < 4 && col < 4);
|
||||
return m_data[col * 4 + row];
|
||||
}
|
||||
inline scalar Matrix::operator()(uint row, uint col) const
|
||||
{
|
||||
nvDebugCheck(row < 4 && col < 4);
|
||||
return m_data[col * 4 + row];
|
||||
}
|
||||
inline scalar & Matrix::operator()(uint row, uint col)
|
||||
{
|
||||
nvDebugCheck(row < 4 && col < 4);
|
||||
return m_data[col * 4 + row];
|
||||
}
|
||||
|
||||
inline const scalar * Matrix::ptr() const
|
||||
{
|
||||
return m_data;
|
||||
}
|
||||
|
||||
inline Vector4 Matrix::row(uint i) const
|
||||
{
|
||||
nvDebugCheck(i < 4);
|
||||
return Vector4(get(i, 0), get(i, 1), get(i, 2), get(i, 3));
|
||||
}
|
||||
|
||||
inline Vector4 Matrix::column(uint i) const
|
||||
{
|
||||
nvDebugCheck(i < 4);
|
||||
return Vector4(get(0, i), get(1, i), get(2, i), get(3, i));
|
||||
}
|
||||
|
||||
/// Apply scale.
|
||||
inline void Matrix::scale(scalar s)
|
||||
{
|
||||
m_data[0] *= s; m_data[1] *= s; m_data[2] *= s; m_data[3] *= s;
|
||||
m_data[4] *= s; m_data[5] *= s; m_data[6] *= s; m_data[7] *= s;
|
||||
m_data[8] *= s; m_data[9] *= s; m_data[10] *= s; m_data[11] *= s;
|
||||
}
|
||||
|
||||
/// Apply scale.
|
||||
inline void Matrix::scale(Vector3::Arg s)
|
||||
{
|
||||
m_data[0] *= s.x(); m_data[1] *= s.x(); m_data[2] *= s.x(); m_data[3] *= s.x();
|
||||
m_data[4] *= s.y(); m_data[5] *= s.y(); m_data[6] *= s.y(); m_data[7] *= s.y();
|
||||
m_data[8] *= s.z(); m_data[9] *= s.z(); m_data[10] *= s.z(); m_data[11] *= s.z();
|
||||
}
|
||||
|
||||
/// Apply translation.
|
||||
inline void Matrix::translate(Vector3::Arg t)
|
||||
{
|
||||
m_data[12] = m_data[0] * t.x() + m_data[4] * t.y() + m_data[8] * t.z() + m_data[12];
|
||||
m_data[13] = m_data[1] * t.x() + m_data[5] * t.y() + m_data[9] * t.z() + m_data[13];
|
||||
m_data[14] = m_data[2] * t.x() + m_data[6] * t.y() + m_data[10] * t.z() + m_data[14];
|
||||
m_data[15] = m_data[3] * t.x() + m_data[7] * t.y() + m_data[11] * t.z() + m_data[15];
|
||||
}
|
||||
|
||||
Matrix rotation(scalar theta, scalar v0, scalar v1, scalar v2);
|
||||
|
||||
/// Apply rotation.
|
||||
inline void Matrix::rotate(scalar theta, scalar v0, scalar v1, scalar v2)
|
||||
{
|
||||
Matrix R(rotation(theta, v0, v1, v2));
|
||||
apply(R);
|
||||
}
|
||||
|
||||
/// Apply transform.
|
||||
inline void Matrix::apply(Matrix::Arg m)
|
||||
{
|
||||
nvDebugCheck(this != &m);
|
||||
|
||||
for(int i = 0; i < 4; i++) {
|
||||
const float ai0 = get(i,0), ai1 = get(i,1), ai2 = get(i,2), ai3 = get(i,3);
|
||||
m_data[0 + i] = ai0 * m(0,0) + ai1 * m(1,0) + ai2 * m(2,0) + ai3 * m(3,0);
|
||||
m_data[4 + i] = ai0 * m(0,1) + ai1 * m(1,1) + ai2 * m(2,1) + ai3 * m(3,1);
|
||||
m_data[8 + i] = ai0 * m(0,2) + ai1 * m(1,2) + ai2 * m(2,2) + ai3 * m(3,2);
|
||||
m_data[12+ i] = ai0 * m(0,3) + ai1 * m(1,3) + ai2 * m(2,3) + ai3 * m(3,3);
|
||||
}
|
||||
}
|
||||
|
||||
/// Get scale matrix.
|
||||
inline Matrix scale(Vector3::Arg s)
|
||||
{
|
||||
Matrix m(identity);
|
||||
m(0,0) = s.x();
|
||||
m(1,1) = s.y();
|
||||
m(2,2) = s.z();
|
||||
return m;
|
||||
}
|
||||
|
||||
/// Get scale matrix.
|
||||
inline Matrix scale(scalar s)
|
||||
{
|
||||
Matrix m(identity);
|
||||
m(0,0) = m(1,1) = m(2,2) = s;
|
||||
return m;
|
||||
}
|
||||
|
||||
/// Get translation matrix.
|
||||
inline Matrix translation(Vector3::Arg t)
|
||||
{
|
||||
Matrix m(identity);
|
||||
m(0,3) = t.x();
|
||||
m(1,3) = t.y();
|
||||
m(2,3) = t.z();
|
||||
return m;
|
||||
}
|
||||
|
||||
/// Get rotation matrix.
|
||||
inline Matrix rotation(scalar theta, scalar v0, scalar v1, scalar v2)
|
||||
{
|
||||
scalar cost = cosf(theta);
|
||||
scalar sint = sinf(theta);
|
||||
|
||||
Matrix m(identity);
|
||||
|
||||
if( 1 == v0 && 0 == v1 && 0 == v2 ) {
|
||||
m(1,1) = cost; m(2,1) = -sint;
|
||||
m(1,2) = sint; m(2,2) = cost;
|
||||
}
|
||||
else if( 0 == v0 && 1 == v1 && 0 == v2 ) {
|
||||
m(0,0) = cost; m(2,0) = sint;
|
||||
m(1,2) = -sint; m(2,2) = cost;
|
||||
}
|
||||
else if( 0 == v0 && 0 == v1 && 1 == v2 ) {
|
||||
m(0,0) = cost; m(1,0) = -sint;
|
||||
m(0,1) = sint; m(1,1) = cost;
|
||||
}
|
||||
else {
|
||||
scalar a2, b2, c2;
|
||||
a2 = v0 * v0;
|
||||
b2 = v1 * v1;
|
||||
c2 = v2 * v2;
|
||||
|
||||
scalar iscale = 1.0f / sqrtf(a2 + b2 + c2);
|
||||
v0 *= iscale;
|
||||
v1 *= iscale;
|
||||
v2 *= iscale;
|
||||
|
||||
scalar abm, acm, bcm;
|
||||
scalar mcos, asin, bsin, csin;
|
||||
mcos = 1.0f - cost;
|
||||
abm = v0 * v1 * mcos;
|
||||
acm = v0 * v2 * mcos;
|
||||
bcm = v1 * v2 * mcos;
|
||||
asin = v0 * sint;
|
||||
bsin = v1 * sint;
|
||||
csin = v2 * sint;
|
||||
m(0,0) = a2 * mcos + cost;
|
||||
m(1,0) = abm - csin;
|
||||
m(2,0) = acm + bsin;
|
||||
m(3,0) = abm + csin;
|
||||
m(1,1) = b2 * mcos + cost;
|
||||
m(2,1) = bcm - asin;
|
||||
m(3,1) = acm - bsin;
|
||||
m(1,2) = bcm + asin;
|
||||
m(2,2) = c2 * mcos + cost;
|
||||
}
|
||||
return m;
|
||||
}
|
||||
|
||||
//Matrix rotation(scalar yaw, scalar pitch, scalar roll);
|
||||
//Matrix skew(scalar angle, Vector3::Arg v1, Vector3::Arg v2);
|
||||
|
||||
/// Get frustum matrix.
|
||||
inline Matrix frustum(scalar xmin, scalar xmax, scalar ymin, scalar ymax, scalar zNear, scalar zFar)
|
||||
{
|
||||
Matrix m(zero);
|
||||
|
||||
scalar doubleznear = 2.0f * zNear;
|
||||
scalar one_deltax = 1.0f / (xmax - xmin);
|
||||
scalar one_deltay = 1.0f / (ymax - ymin);
|
||||
scalar one_deltaz = 1.0f / (zFar - zNear);
|
||||
|
||||
m(0,0) = doubleznear * one_deltax;
|
||||
m(1,1) = doubleznear * one_deltay;
|
||||
m(0,2) = (xmax + xmin) * one_deltax;
|
||||
m(1,2) = (ymax + ymin) * one_deltay;
|
||||
m(2,2) = -(zFar + zNear) * one_deltaz;
|
||||
m(3,2) = -1.0f;
|
||||
m(2,3) = -(zFar * doubleznear) * one_deltaz;
|
||||
|
||||
return m;
|
||||
}
|
||||
|
||||
/// Get infinite frustum matrix.
|
||||
inline Matrix frustum(scalar xmin, scalar xmax, scalar ymin, scalar ymax, scalar zNear)
|
||||
{
|
||||
Matrix m(zero);
|
||||
|
||||
scalar doubleznear = 2.0f * zNear;
|
||||
scalar one_deltax = 1.0f / (xmax - xmin);
|
||||
scalar one_deltay = 1.0f / (ymax - ymin);
|
||||
scalar nudge = 1.0; // 0.999;
|
||||
|
||||
m(0,0) = doubleznear * one_deltax;
|
||||
m(1,1) = doubleznear * one_deltay;
|
||||
m(0,2) = (xmax + xmin) * one_deltax;
|
||||
m(1,2) = (ymax + ymin) * one_deltay;
|
||||
m(2,2) = -1.0f * nudge;
|
||||
m(3,2) = -1.0f;
|
||||
m(2,3) = -doubleznear * nudge;
|
||||
|
||||
return m;
|
||||
}
|
||||
|
||||
/// Get perspective matrix.
|
||||
inline Matrix perspective(scalar fovy, scalar aspect, scalar zNear, scalar zFar)
|
||||
{
|
||||
scalar xmax = zNear * tan(fovy / 2);
|
||||
scalar xmin = -xmax;
|
||||
|
||||
scalar ymax = xmax / aspect;
|
||||
scalar ymin = -ymax;
|
||||
|
||||
return frustum(xmin, xmax, ymin, ymax, zNear, zFar);
|
||||
}
|
||||
|
||||
/// Get infinite perspective matrix.
|
||||
inline Matrix perspective(scalar fovy, scalar aspect, scalar zNear)
|
||||
{
|
||||
scalar x = zNear * tan(fovy / 2);
|
||||
scalar y = x / aspect;
|
||||
return frustum( -x, x, -y, y, zNear );
|
||||
}
|
||||
|
||||
/// Get matrix determinant.
|
||||
inline scalar determinant(Matrix::Arg m)
|
||||
{
|
||||
// @@ not sure this is correct.
|
||||
return m(0,0) * m(1,1) * m(2,2) * m(3,3) -
|
||||
m(1,0) * m(2,1) * m(3,2) * m(0,3) +
|
||||
m(2,0) * m(3,1) * m(0,2) * m(1,3) -
|
||||
m(3,0) * m(0,1) * m(1,2) * m(2,3) -
|
||||
m(3,0) * m(2,1) * m(1,2) * m(0,3) +
|
||||
m(2,0) * m(1,1) * m(0,2) * m(3,3) -
|
||||
m(1,0) * m(0,1) * m(3,2) * m(2,3) +
|
||||
m(0,0) * m(3,1) * m(2,2) * m(1,3);
|
||||
}
|
||||
|
||||
//inline Matrix transpose(Matrix::Arg m)
|
||||
//{
|
||||
//}
|
||||
|
||||
//Matrix inverse(Matrix::Arg m);
|
||||
//Matrix isometryInverse(Matrix::Arg m);
|
||||
//Matrix affineInverse(Matrix::Arg m);
|
||||
|
||||
/// Transform the given 3d point with the given matrix.
|
||||
inline Vector3 transformPoint(Matrix::Arg m, Vector3::Arg p)
|
||||
{
|
||||
return Vector3(
|
||||
p.x() * m(0,0) + p.y() * m(0,1) + p.z() * m(0,2) + m(0,3),
|
||||
p.x() * m(1,0) + p.y() * m(1,1) + p.z() * m(1,2) + m(1,3),
|
||||
p.x() * m(2,0) + p.y() * m(2,1) + p.z() * m(2,2) + m(2,3));
|
||||
}
|
||||
|
||||
/// Transform the given 3d vector with the given matrix.
|
||||
inline Vector3 transformVector(Matrix::Arg m, Vector3::Arg p)
|
||||
{
|
||||
return Vector3(
|
||||
p.x() * m(0,0) + p.y() * m(0,1) + p.z() * m(0,2),
|
||||
p.x() * m(1,0) + p.y() * m(1,1) + p.z() * m(1,2),
|
||||
p.x() * m(2,0) + p.y() * m(2,1) + p.z() * m(2,2));
|
||||
}
|
||||
|
||||
/// Transform the given 4d vector with the given matrix.
|
||||
inline Vector4 transform(Matrix::Arg m, Vector4::Arg p)
|
||||
{
|
||||
return Vector4(
|
||||
p.x() * m(0,0) + p.y() * m(0,1) + p.z() * m(0,2) + p.w() * m(0,3),
|
||||
p.x() * m(1,0) + p.y() * m(1,1) + p.z() * m(1,2) + p.w() * m(1,3),
|
||||
p.x() * m(2,0) + p.y() * m(2,1) + p.z() * m(2,2) + p.w() * m(2,3),
|
||||
p.x() * m(3,0) + p.y() * m(3,1) + p.z() * m(3,2) + p.w() * m(3,3));
|
||||
}
|
||||
|
||||
|
||||
} // nv namespace
|
||||
|
||||
|
||||
|
||||
|
||||
#if 0
|
||||
/** @name Special matrices. */
|
||||
//@{
|
||||
/** Generate a translation matrix. */
|
||||
void TranslationMatrix(const Vec3 & v) {
|
||||
data[0] = 1; data[1] = 0; data[2] = 0; data[3] = 0;
|
||||
data[4] = 0; data[5] = 1; data[6] = 0; data[7] = 0;
|
||||
data[8] = 0; data[9] = 0; data[10] = 1; data[11] = 0;
|
||||
data[12] = v.x; data[13] = v.y; data[14] = v.z; data[15] = 1;
|
||||
}
|
||||
|
||||
/** Rotate theta degrees around v. */
|
||||
void RotationMatrix( scalar theta, scalar v0, scalar v1, scalar v2 ) {
|
||||
scalar cost = cos(theta);
|
||||
scalar sint = sin(theta);
|
||||
|
||||
if( 1 == v0 && 0 == v1 && 0 == v2 ) {
|
||||
data[0] = 1.0f; data[1] = 0.0f; data[2] = 0.0f; data[3] = 0.0f;
|
||||
data[4] = 0.0f; data[5] = cost; data[6] = -sint;data[7] = 0.0f;
|
||||
data[8] = 0.0f; data[9] = sint; data[10] = cost;data[11] = 0.0f;
|
||||
data[12] = 0.0f;data[13] = 0.0f;data[14] = 0.0f;data[15] = 1.0f;
|
||||
}
|
||||
else if( 0 == v0 && 1 == v1 && 0 == v2 ) {
|
||||
data[0] = cost; data[1] = 0.0f; data[2] = sint; data[3] = 0.0f;
|
||||
data[4] = 0.0f; data[5] = 1.0f; data[6] = 0.0f; data[7] = 0.0f;
|
||||
data[8] = -sint;data[9] = 0.0f;data[10] = cost; data[11] = 0.0f;
|
||||
data[12] = 0.0f;data[13] = 0.0f;data[14] = 0.0f;data[15] = 1.0f;
|
||||
}
|
||||
else if( 0 == v0 && 0 == v1 && 1 == v2 ) {
|
||||
data[0] = cost; data[1] = -sint;data[2] = 0.0f; data[3] = 0.0f;
|
||||
data[4] = sint; data[5] = cost; data[6] = 0.0f; data[7] = 0.0f;
|
||||
data[8] = 0.0f; data[9] = 0.0f; data[10] = 1.0f;data[11] = 0.0f;
|
||||
data[12] = 0.0f;data[13] = 0.0f;data[14] = 0.0f;data[15] = 1.0f;
|
||||
}
|
||||
else {
|
||||
//we need scale a,b,c to unit length.
|
||||
scalar a2, b2, c2;
|
||||
a2 = v0 * v0;
|
||||
b2 = v1 * v1;
|
||||
c2 = v2 * v2;
|
||||
|
||||
scalar iscale = 1.0f / sqrtf(a2 + b2 + c2);
|
||||
v0 *= iscale;
|
||||
v1 *= iscale;
|
||||
v2 *= iscale;
|
||||
|
||||
scalar abm, acm, bcm;
|
||||
scalar mcos, asin, bsin, csin;
|
||||
mcos = 1.0f - cost;
|
||||
abm = v0 * v1 * mcos;
|
||||
acm = v0 * v2 * mcos;
|
||||
bcm = v1 * v2 * mcos;
|
||||
asin = v0 * sint;
|
||||
bsin = v1 * sint;
|
||||
csin = v2 * sint;
|
||||
data[0] = a2 * mcos + cost;
|
||||
data[1] = abm - csin;
|
||||
data[2] = acm + bsin;
|
||||
data[3] = abm + csin;
|
||||
data[4] = 0.0f;
|
||||
data[5] = b2 * mcos + cost;
|
||||
data[6] = bcm - asin;
|
||||
data[7] = acm - bsin;
|
||||
data[8] = 0.0f;
|
||||
data[9] = bcm + asin;
|
||||
data[10] = c2 * mcos + cost;
|
||||
data[11] = 0.0f;
|
||||
data[12] = 0.0f;
|
||||
data[13] = 0.0f;
|
||||
data[14] = 0.0f;
|
||||
data[15] = 1.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
void SkewMatrix(scalar angle, const Vec3 & v1, const Vec3 & v2) {
|
||||
v1.Normalize();
|
||||
v2.Normalize();
|
||||
|
||||
Vec3 v3;
|
||||
v3.Cross(v1, v2);
|
||||
v3.Normalize();
|
||||
|
||||
// Get skew factor.
|
||||
scalar costheta = Vec3DotProduct(v1, v2);
|
||||
scalar sintheta = Real.Sqrt(1 - costheta * costheta);
|
||||
scalar skew = tan(Trig.DegreesToRadians(angle) + acos(sintheta)) * sintheta - costheta;
|
||||
|
||||
// Build orthonormal matrix.
|
||||
v1 = FXVector3.Cross(v3, v2);
|
||||
v1.Normalize();
|
||||
|
||||
Matrix R = Matrix::Identity;
|
||||
R[0, 0] = v3.X;<3B>// Not sure this is in the correct order...
|
||||
R[1, 0] = v3.Y;
|
||||
R[2, 0] = v3.Z;
|
||||
R[0, 1] = v1.X;
|
||||
R[1, 1] = v1.Y;
|
||||
R[2, 1] = v1.Z;
|
||||
R[0, 2] = v2.X;
|
||||
R[1, 2] = v2.Y;
|
||||
R[2, 2] = v2.Z;
|
||||
|
||||
// Build skew matrix.
|
||||
Matrix S = Matrix::Identity;
|
||||
S[2, 1] = -skew;
|
||||
|
||||
// Return skew transform.
|
||||
return R * S * R.Transpose; // Not sure this is in the correct order...
|
||||
}
|
||||
*/
|
||||
|
||||
/**
|
||||
* Generate rotation matrix for the euler angles. This is the same as computing
|
||||
* 3 rotation matrices and multiplying them together in our custom order.
|
||||
*
|
||||
* @todo Have to recompute this code for our new convention.
|
||||
**/
|
||||
void RotationMatrix( scalar yaw, scalar pitch, scalar roll ) {
|
||||
scalar sy = sin(yaw+ToRadian(90));
|
||||
scalar cy = cos(yaw+ToRadian(90));
|
||||
scalar sp = sin(pitch-ToRadian(90));
|
||||
scalar cp = cos(pitch-ToRadian(90));
|
||||
scalar sr = sin(roll);
|
||||
scalar cr = cos(roll);
|
||||
|
||||
data[0] = cr*cy + sr*sp*sy;
|
||||
data[1] = cp*sy;
|
||||
data[2] = -sr*cy + cr*sp*sy;
|
||||
data[3] = 0;
|
||||
|
||||
data[4] = -cr*sy + sr*sp*cy;
|
||||
data[5] = cp*cy;
|
||||
data[6] = sr*sy + cr*sp*cy;
|
||||
data[7] = 0;
|
||||
|
||||
data[8] = sr*cp;
|
||||
data[9] = -sp;
|
||||
data[10] = cr*cp;
|
||||
data[11] = 0;
|
||||
|
||||
data[12] = 0;
|
||||
data[13] = 0;
|
||||
data[14] = 0;
|
||||
data[15] = 1;
|
||||
}
|
||||
|
||||
/** Create a frustum matrix with the far plane at the infinity. */
|
||||
void Frustum( scalar xmin, scalar xmax, scalar ymin, scalar ymax, scalar zNear, scalar zFar ) {
|
||||
scalar one_deltax, one_deltay, one_deltaz, doubleznear;
|
||||
|
||||
doubleznear = 2.0f * zNear;
|
||||
one_deltax = 1.0f / (xmax - xmin);
|
||||
one_deltay = 1.0f / (ymax - ymin);
|
||||
one_deltaz = 1.0f / (zFar - zNear);
|
||||
|
||||
data[0] = (scalar)(doubleznear * one_deltax);
|
||||
data[1] = 0.0f;
|
||||
data[2] = 0.0f;
|
||||
data[3] = 0.0f;
|
||||
data[4] = 0.0f;
|
||||
data[5] = (scalar)(doubleznear * one_deltay);
|
||||
data[6] = 0.f;
|
||||
data[7] = 0.f;
|
||||
data[8] = (scalar)((xmax + xmin) * one_deltax);
|
||||
data[9] = (scalar)((ymax + ymin) * one_deltay);
|
||||
data[10] = (scalar)(-(zFar + zNear) * one_deltaz);
|
||||
data[11] = -1.f;
|
||||
data[12] = 0.f;
|
||||
data[13] = 0.f;
|
||||
data[14] = (scalar)(-(zFar * doubleznear) * one_deltaz);
|
||||
data[15] = 0.f;
|
||||
}
|
||||
|
||||
/** Create a frustum matrix with the far plane at the infinity. */
|
||||
void FrustumInf( scalar xmin, scalar xmax, scalar ymin, scalar ymax, scalar zNear ) {
|
||||
scalar one_deltax, one_deltay, doubleznear, nudge;
|
||||
|
||||
doubleznear = 2.0f * zNear;
|
||||
one_deltax = 1.0f / (xmax - xmin);
|
||||
one_deltay = 1.0f / (ymax - ymin);
|
||||
nudge = 1.0; // 0.999;
|
||||
|
||||
data[0] = doubleznear * one_deltax;
|
||||
data[1] = 0.0f;
|
||||
data[2] = 0.0f;
|
||||
data[3] = 0.0f;
|
||||
|
||||
data[4] = 0.0f;
|
||||
data[5] = doubleznear * one_deltay;
|
||||
data[6] = 0.f;
|
||||
data[7] = 0.f;
|
||||
|
||||
data[8] = (xmax + xmin) * one_deltax;
|
||||
data[9] = (ymax + ymin) * one_deltay;
|
||||
data[10] = -1.0f * nudge;
|
||||
data[11] = -1.0f;
|
||||
|
||||
data[12] = 0.f;
|
||||
data[13] = 0.f;
|
||||
data[14] = -doubleznear * nudge;
|
||||
data[15] = 0.f;
|
||||
}
|
||||
|
||||
/** Create an inverse frustum matrix with the far plane at the infinity. */
|
||||
void FrustumInfInv( scalar left, scalar right, scalar bottom, scalar top, scalar zNear ) {
|
||||
// this matrix is wrong (not tested scalarly) I think it should be transposed.
|
||||
data[0] = (right - left) / (2 * zNear);
|
||||
data[1] = 0;
|
||||
data[2] = 0;
|
||||
data[3] = (right + left) / (2 * zNear);
|
||||
data[4] = 0;
|
||||
data[5] = (top - bottom) / (2 * zNear);
|
||||
data[6] = 0;
|
||||
data[7] = (top + bottom) / (2 * zNear);
|
||||
data[8] = 0;
|
||||
data[9] = 0;
|
||||
data[10] = 0;
|
||||
data[11] = -1;
|
||||
data[12] = 0;
|
||||
data[13] = 0;
|
||||
data[14] = -1 / (2 * zNear);
|
||||
data[15] = 1 / (2 * zNear);
|
||||
}
|
||||
|
||||
/** Create an homogeneous projection matrix. */
|
||||
void Perspective( scalar fov, scalar aspect, scalar zNear, scalar zFar ) {
|
||||
scalar xmin, xmax, ymin, ymax;
|
||||
|
||||
xmax = zNear * tan( fov/2 );
|
||||
xmin = -xmax;
|
||||
|
||||
ymax = xmax / aspect;
|
||||
ymin = -ymax;
|
||||
|
||||
Frustum(xmin, xmax, ymin, ymax, zNear, zFar);
|
||||
}
|
||||
|
||||
/** Create a projection matrix with the far plane at the infinity. */
|
||||
void PerspectiveInf( scalar fov, scalar aspect, scalar zNear ) {
|
||||
scalar x = zNear * tan( fov/2 );
|
||||
scalar y = x / aspect;
|
||||
FrustumInf( -x, x, -y, y, zNear );
|
||||
}
|
||||
|
||||
/** Create an inverse projection matrix with far plane at the infinity. */
|
||||
void PerspectiveInfInv( scalar fov, scalar aspect, scalar zNear ) {
|
||||
scalar x = zNear * tan( fov/2 );
|
||||
scalar y = x / aspect;
|
||||
FrustumInfInv( -x, x, -y, y, zNear );
|
||||
}
|
||||
|
||||
/** Build bone matrix from quatertion and offset. */
|
||||
void BoneMatrix(const Quat & q, const Vec3 & offset) {
|
||||
scalar x2, y2, z2, xx, xy, xz, yy, yz, zz, wx, wy, wz;
|
||||
|
||||
// calculate coefficients
|
||||
x2 = q.x + q.x;
|
||||
y2 = q.y + q.y;
|
||||
z2 = q.z + q.z;
|
||||
|
||||
xx = q.x * x2; xy = q.x * y2; xz = q.x * z2;
|
||||
yy = q.y * y2; yz = q.y * z2; zz = q.z * z2;
|
||||
wx = q.w * x2; wy = q.w * y2; wz = q.w * z2;
|
||||
|
||||
data[0] = 1.0f - (yy + zz);
|
||||
data[1] = xy - wz;
|
||||
data[2] = xz + wy;
|
||||
data[3] = 0.0f;
|
||||
|
||||
data[4] = xy + wz;
|
||||
data[5] = 1.0f - (xx + zz);
|
||||
data[6] = yz - wx;
|
||||
data[7] = 0.0f;
|
||||
|
||||
data[8] = xz - wy;
|
||||
data[9] = yz + wx;
|
||||
data[10] = 1.0f - (xx + yy);
|
||||
data[11] = 0.0f;
|
||||
|
||||
data[12] = offset.x;
|
||||
data[13] = offset.y;
|
||||
data[14] = offset.z;
|
||||
data[15] = 1.0f;
|
||||
}
|
||||
|
||||
//@}
|
||||
|
||||
|
||||
/** @name Transformations: */
|
||||
//@{
|
||||
|
||||
/** Apply a general scale. */
|
||||
void Scale( scalar x, scalar y, scalar z ) {
|
||||
data[0] *= x; data[4] *= y; data[8] *= z;
|
||||
data[1] *= x; data[5] *= y; data[9] *= z;
|
||||
data[2] *= x; data[6] *= y; data[10] *= z;
|
||||
data[3] *= x; data[7] *= y; data[11] *= z;
|
||||
}
|
||||
|
||||
/** Apply a rotation of theta degrees around the axis v*/
|
||||
void Rotate( scalar theta, const Vec3 & v ) {
|
||||
Matrix b;
|
||||
b.RotationMatrix( theta, v[0], v[1], v[2] );
|
||||
Multiply4x3( b );
|
||||
}
|
||||
|
||||
/** Apply a rotation of theta degrees around the axis v*/
|
||||
void Rotate( scalar theta, scalar v0, scalar v1, scalar v2 ) {
|
||||
Matrix b;
|
||||
b.RotationMatrix( theta, v0, v1, v2 );
|
||||
Multiply4x3( b );
|
||||
}
|
||||
|
||||
/**
|
||||
* Translate the matrix by t. This is the same as multiplying by a
|
||||
* translation matrix with the given offset.
|
||||
* this = T * this
|
||||
*/
|
||||
void Translate( const Vec3 &t ) {
|
||||
data[12] = data[0] * t.x + data[4] * t.y + data[8] * t.z + data[12];
|
||||
data[13] = data[1] * t.x + data[5] * t.y + data[9] * t.z + data[13];
|
||||
data[14] = data[2] * t.x + data[6] * t.y + data[10] * t.z + data[14];
|
||||
data[15] = data[3] * t.x + data[7] * t.y + data[11] * t.z + data[15];
|
||||
}
|
||||
|
||||
/**
|
||||
* Translate the matrix by x, y, z. This is the same as multiplying by a
|
||||
* translation matrix with the given offsets.
|
||||
*/
|
||||
void Translate( scalar x, scalar y, scalar z ) {
|
||||
data[12] = data[0] * x + data[4] * y + data[8] * z + data[12];
|
||||
data[13] = data[1] * x + data[5] * y + data[9] * z + data[13];
|
||||
data[14] = data[2] * x + data[6] * y + data[10] * z + data[14];
|
||||
data[15] = data[3] * x + data[7] * y + data[11] * z + data[15];
|
||||
}
|
||||
|
||||
/** Compute the transposed matrix. */
|
||||
void Transpose() {
|
||||
piSwap(data[1], data[4]);
|
||||
piSwap(data[2], data[8]);
|
||||
piSwap(data[6], data[9]);
|
||||
piSwap(data[3], data[12]);
|
||||
piSwap(data[7], data[13]);
|
||||
piSwap(data[11], data[14]);
|
||||
}
|
||||
|
||||
/** Compute the inverse of a rigid-body/isometry/orthonormal matrix. */
|
||||
void IsometryInverse() {
|
||||
// transposed 3x3 upper left matrix
|
||||
piSwap(data[1], data[4]);
|
||||
piSwap(data[2], data[8]);
|
||||
piSwap(data[6], data[9]);
|
||||
|
||||
// translate by the negative offsets
|
||||
Vec3 v(-data[12], -data[13], -data[14]);
|
||||
data[12] = data[13] = data[14] = 0;
|
||||
Translate(v);
|
||||
}
|
||||
|
||||
/** Compute the inverse of the affine portion of this matrix. */
|
||||
void AffineInverse() {
|
||||
data[12] = data[13] = data[14] = 0;
|
||||
Transpose();
|
||||
}
|
||||
//@}
|
||||
|
||||
/** @name Matrix operations: */
|
||||
//@{
|
||||
|
||||
/** Return the determinant of this matrix. */
|
||||
scalar Determinant() const {
|
||||
return data[0] * data[5] * data[10] * data[15] +
|
||||
data[1] * data[6] * data[11] * data[12] +
|
||||
data[2] * data[7] * data[ 8] * data[13] +
|
||||
data[3] * data[4] * data[ 9] * data[14] -
|
||||
data[3] * data[6] * data[ 9] * data[12] -
|
||||
data[2] * data[5] * data[ 8] * data[15] -
|
||||
data[1] * data[4] * data[11] * data[14] -
|
||||
data[0] * data[7] * data[10] * data[12];
|
||||
}
|
||||
|
||||
|
||||
/** Standard matrix product: this *= B. */
|
||||
void Multiply4x4( const Matrix & restrict B ) {
|
||||
Multiply4x4(*this, B);
|
||||
}
|
||||
|
||||
/** Standard matrix product: this = A * B. this != B*/
|
||||
void Multiply4x4( const Matrix & A, const Matrix & restrict B ) {
|
||||
piDebugCheck(this != &B);
|
||||
|
||||
for(int i = 0; i < 4; i++) {
|
||||
const scalar ai0 = A(i,0), ai1 = A(i,1), ai2 = A(i,2), ai3 = A(i,3);
|
||||
GetElem(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0) + ai3 * B(3,0);
|
||||
GetElem(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1) + ai3 * B(3,1);
|
||||
GetElem(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2) + ai3 * B(3,2);
|
||||
GetElem(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3 * B(3,3);
|
||||
}
|
||||
|
||||
/* Unrolled but does not allow this == A
|
||||
data[0] = A.data[0] * B.data[0] + A.data[4] * B.data[1] + A.data[8] * B.data[2] + A.data[12] * B.data[3];
|
||||
data[1] = A.data[1] * B.data[0] + A.data[5] * B.data[1] + A.data[9] * B.data[2] + A.data[13] * B.data[3];
|
||||
data[2] = A.data[2] * B.data[0] + A.data[6] * B.data[1] + A.data[10] * B.data[2] + A.data[14] * B.data[3];
|
||||
data[3] = A.data[3] * B.data[0] + A.data[7] * B.data[1] + A.data[11] * B.data[2] + A.data[15] * B.data[3];
|
||||
data[4] = A.data[0] * B.data[4] + A.data[4] * B.data[5] + A.data[8] * B.data[6] + A.data[12] * B.data[7];
|
||||
data[5] = A.data[1] * B.data[4] + A.data[5] * B.data[5] + A.data[9] * B.data[6] + A.data[13] * B.data[7];
|
||||
data[6] = A.data[2] * B.data[4] + A.data[6] * B.data[5] + A.data[10] * B.data[6] + A.data[14] * B.data[7];
|
||||
data[7] = A.data[3] * B.data[4] + A.data[7] * B.data[5] + A.data[11] * B.data[6] + A.data[15] * B.data[7];
|
||||
data[8] = A.data[0] * B.data[8] + A.data[4] * B.data[9] + A.data[8] * B.data[10] + A.data[12] * B.data[11];
|
||||
data[9] = A.data[1] * B.data[8] + A.data[5] * B.data[9] + A.data[9] * B.data[10] + A.data[13] * B.data[11];
|
||||
data[10]= A.data[2] * B.data[8] + A.data[6] * B.data[9] + A.data[10] * B.data[10] + A.data[14] * B.data[11];
|
||||
data[11]= A.data[3] * B.data[8] + A.data[7] * B.data[9] + A.data[11] * B.data[10] + A.data[15] * B.data[11];
|
||||
data[12]= A.data[0] * B.data[12] + A.data[4] * B.data[13] + A.data[8] * B.data[14] + A.data[12] * B.data[15];
|
||||
data[13]= A.data[1] * B.data[12] + A.data[5] * B.data[13] + A.data[9] * B.data[14] + A.data[13] * B.data[15];
|
||||
data[14]= A.data[2] * B.data[12] + A.data[6] * B.data[13] + A.data[10] * B.data[14] + A.data[14] * B.data[15];
|
||||
data[15]= A.data[3] * B.data[12] + A.data[7] * B.data[13] + A.data[11] * B.data[14] + A.data[15] * B.data[15];
|
||||
*/
|
||||
}
|
||||
|
||||
/** Standard matrix product: this *= B. */
|
||||
void Multiply4x3( const Matrix & restrict B ) {
|
||||
Multiply4x3(*this, B);
|
||||
}
|
||||
|
||||
/** Standard product of matrices, where the last row is [0 0 0 1]. */
|
||||
void Multiply4x3( const Matrix & A, const Matrix & restrict B ) {
|
||||
piDebugCheck(this != &B);
|
||||
|
||||
for(int i = 0; i < 3; i++) {
|
||||
const scalar ai0 = A(i,0), ai1 = A(i,1), ai2 = A(i,2), ai3 = A(i,3);
|
||||
GetElem(i,0) = ai0 * B(0,0) + ai1 * B(1,0) + ai2 * B(2,0) + ai3 * B(3,0);
|
||||
GetElem(i,1) = ai0 * B(0,1) + ai1 * B(1,1) + ai2 * B(2,1) + ai3 * B(3,1);
|
||||
GetElem(i,2) = ai0 * B(0,2) + ai1 * B(1,2) + ai2 * B(2,2) + ai3 * B(3,2);
|
||||
GetElem(i,3) = ai0 * B(0,3) + ai1 * B(1,3) + ai2 * B(2,3) + ai3 * B(3,3);
|
||||
}
|
||||
data[3] = 0.0f; data[7] = 0.0f; data[11] = 0.0f; data[15] = 1.0f;
|
||||
|
||||
/* Unrolled but does not allow this == A
|
||||
data[0] = a.data[0] * b.data[0] + a.data[4] * b.data[1] + a.data[8] * b.data[2] + a.data[12] * b.data[3];
|
||||
data[1] = a.data[1] * b.data[0] + a.data[5] * b.data[1] + a.data[9] * b.data[2] + a.data[13] * b.data[3];
|
||||
data[2] = a.data[2] * b.data[0] + a.data[6] * b.data[1] + a.data[10] * b.data[2] + a.data[14] * b.data[3];
|
||||
data[3] = 0.0f;
|
||||
data[4] = a.data[0] * b.data[4] + a.data[4] * b.data[5] + a.data[8] * b.data[6] + a.data[12] * b.data[7];
|
||||
data[5] = a.data[1] * b.data[4] + a.data[5] * b.data[5] + a.data[9] * b.data[6] + a.data[13] * b.data[7];
|
||||
data[6] = a.data[2] * b.data[4] + a.data[6] * b.data[5] + a.data[10] * b.data[6] + a.data[14] * b.data[7];
|
||||
data[7] = 0.0f;
|
||||
data[8] = a.data[0] * b.data[8] + a.data[4] * b.data[9] + a.data[8] * b.data[10] + a.data[12] * b.data[11];
|
||||
data[9] = a.data[1] * b.data[8] + a.data[5] * b.data[9] + a.data[9] * b.data[10] + a.data[13] * b.data[11];
|
||||
data[10]= a.data[2] * b.data[8] + a.data[6] * b.data[9] + a.data[10] * b.data[10] + a.data[14] * b.data[11];
|
||||
data[11]= 0.0f;
|
||||
data[12]= a.data[0] * b.data[12] + a.data[4] * b.data[13] + a.data[8] * b.data[14] + a.data[12] * b.data[15];
|
||||
data[13]= a.data[1] * b.data[12] + a.data[5] * b.data[13] + a.data[9] * b.data[14] + a.data[13] * b.data[15];
|
||||
data[14]= a.data[2] * b.data[12] + a.data[6] * b.data[13] + a.data[10] * b.data[14] + a.data[14] * b.data[15];
|
||||
data[15]= 1.0f;
|
||||
*/
|
||||
}
|
||||
//@}
|
||||
|
||||
|
||||
/** @name Vector operations: */
|
||||
//@{
|
||||
|
||||
/** Transform 3d vector (w=0). */
|
||||
void TransformVec3(const Vec3 & restrict orig, Vec3 * restrict dest) const {
|
||||
piDebugCheck(&orig != dest);
|
||||
dest->x = orig.x * data[0] + orig.y * data[4] + orig.z * data[8];
|
||||
dest->y = orig.x * data[1] + orig.y * data[5] + orig.z * data[9];
|
||||
dest->z = orig.x * data[2] + orig.y * data[6] + orig.z * data[10];
|
||||
}
|
||||
/** Transform 3d vector by the transpose (w=0). */
|
||||
void TransformVec3T(const Vec3 & restrict orig, Vec3 * restrict dest) const {
|
||||
piDebugCheck(&orig != dest);
|
||||
dest->x = orig.x * data[0] + orig.y * data[1] + orig.z * data[2];
|
||||
dest->y = orig.x * data[4] + orig.y * data[5] + orig.z * data[6];
|
||||
dest->z = orig.x * data[8] + orig.y * data[9] + orig.z * data[10];
|
||||
}
|
||||
|
||||
/** Transform a 3d homogeneous vector, where the fourth coordinate is assumed to be 1. */
|
||||
void TransformPoint(const Vec3 & restrict orig, Vec3 * restrict dest) const {
|
||||
piDebugCheck(&orig != dest);
|
||||
dest->x = orig.x * data[0] + orig.y * data[4] + orig.z * data[8] + data[12];
|
||||
dest->y = orig.x * data[1] + orig.y * data[5] + orig.z * data[9] + data[13];
|
||||
dest->z = orig.x * data[2] + orig.y * data[6] + orig.z * data[10] + data[14];
|
||||
}
|
||||
|
||||
/** Transform a point, normalize it, and return w. */
|
||||
scalar TransformPointAndNormalize(const Vec3 & restrict orig, Vec3 * restrict dest) const {
|
||||
piDebugCheck(&orig != dest);
|
||||
scalar w;
|
||||
dest->x = orig.x * data[0] + orig.y * data[4] + orig.z * data[8] + data[12];
|
||||
dest->y = orig.x * data[1] + orig.y * data[5] + orig.z * data[9] + data[13];
|
||||
dest->z = orig.x * data[2] + orig.y * data[6] + orig.z * data[10] + data[14];
|
||||
w = 1 / (orig.x * data[3] + orig.y * data[7] + orig.z * data[11] + data[15]);
|
||||
*dest *= w;
|
||||
return w;
|
||||
}
|
||||
|
||||
/** Transform a point and return w. */
|
||||
scalar TransformPointReturnW(const Vec3 & restrict orig, Vec3 * restrict dest) const {
|
||||
piDebugCheck(&orig != dest);
|
||||
dest->x = orig.x * data[0] + orig.y * data[4] + orig.z * data[8] + data[12];
|
||||
dest->y = orig.x * data[1] + orig.y * data[5] + orig.z * data[9] + data[13];
|
||||
dest->z = orig.x * data[2] + orig.y * data[6] + orig.z * data[10] + data[14];
|
||||
return orig.x * data[3] + orig.y * data[7] + orig.z * data[11] + data[15];
|
||||
}
|
||||
|
||||
/** Transform a normalized 3d point by a 4d matrix and return the resulting 4d vector. */
|
||||
void TransformVec4(const Vec3 & orig, Vec4 * dest) const {
|
||||
dest->x = orig.x * data[0] + orig.y * data[4] + orig.z * data[8] + data[12];
|
||||
dest->y = orig.x * data[1] + orig.y * data[5] + orig.z * data[9] + data[13];
|
||||
dest->z = orig.x * data[2] + orig.y * data[6] + orig.z * data[10] + data[14];
|
||||
dest->w = orig.x * data[3] + orig.y * data[7] + orig.z * data[11] + data[15];
|
||||
}
|
||||
//@}
|
||||
|
||||
/** @name Matrix analysis. */
|
||||
//@{
|
||||
|
||||
/** Get the ZYZ euler angles from the matrix. Assumes the matrix is orthonormal. */
|
||||
void GetEulerAnglesZYZ(scalar * s, scalar * t, scalar * r) const {
|
||||
if( GetElem(2,2) < 1.0f ) {
|
||||
if( GetElem(2,2) > -1.0f ) {
|
||||
// cs*ct*cr-ss*sr -ss*ct*cr-cs*sr st*cr
|
||||
// cs*ct*sr+ss*cr -ss*ct*sr+cs*cr st*sr
|
||||
// -cs*st ss*st ct
|
||||
*s = atan2(GetElem(1,2), -GetElem(0,2));
|
||||
*t = acos(GetElem(2,2));
|
||||
*r = atan2(GetElem(2,1), GetElem(2,0));
|
||||
}
|
||||
else {
|
||||
// -c(s-r) s(s-r) 0
|
||||
// s(s-r) c(s-r) 0
|
||||
// 0 0 -1
|
||||
*s = atan2(GetElem(0, 1), -GetElem(0, 0)); // = s-r
|
||||
*t = PI;
|
||||
*r = 0;
|
||||
}
|
||||
}
|
||||
else {
|
||||
// c(s+r) -s(s+r) 0
|
||||
// s(s+r) c(s+r) 0
|
||||
// 0 0 1
|
||||
*s = atan2(GetElem(0, 1), GetElem(0, 0)); // = s+r
|
||||
*t = 0;
|
||||
*r = 0;
|
||||
}
|
||||
}
|
||||
|
||||
//@}
|
||||
|
||||
MATHLIB_API friend PiStream & operator<< ( PiStream & s, Matrix & m );
|
||||
|
||||
/** Print to debug output. */
|
||||
void Print() const {
|
||||
piDebug( "[ %5.2f %5.2f %5.2f %5.2f ]\n", data[0], data[4], data[8], data[12] );
|
||||
piDebug( "[ %5.2f %5.2f %5.2f %5.2f ]\n", data[1], data[5], data[9], data[13] );
|
||||
piDebug( "[ %5.2f %5.2f %5.2f %5.2f ]\n", data[2], data[6], data[10], data[14] );
|
||||
piDebug( "[ %5.2f %5.2f %5.2f %5.2f ]\n", data[3], data[7], data[11], data[15] );
|
||||
}
|
||||
|
||||
|
||||
public:
|
||||
|
||||
scalar data[16];
|
||||
|
||||
};
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
#endif // NV_MATH_MATRIX_H
|
||||
702
src/nvmath/Vector.h
Normal file
702
src/nvmath/Vector.h
Normal file
@ -0,0 +1,702 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_VECTOR_H
|
||||
#define NV_MATH_VECTOR_H
|
||||
|
||||
#include <nvmath/nvmath.h>
|
||||
#include <nvcore/Containers.h> // min, max
|
||||
|
||||
namespace nv
|
||||
{
|
||||
|
||||
enum zero_t { zero };
|
||||
enum identity_t { identity };
|
||||
|
||||
// I should probably use templates.
|
||||
typedef float scalar;
|
||||
|
||||
class NVMATH_CLASS Vector2
|
||||
{
|
||||
public:
|
||||
typedef Vector2 const & Arg;
|
||||
|
||||
Vector2();
|
||||
explicit Vector2(zero_t);
|
||||
Vector2(scalar x, scalar y);
|
||||
Vector2(Vector2::Arg v);
|
||||
|
||||
const Vector2 & operator=(Vector2::Arg v);
|
||||
|
||||
scalar x() const;
|
||||
scalar y() const;
|
||||
|
||||
void set(scalar x, scalar y);
|
||||
|
||||
Vector2 operator-() const;
|
||||
void operator+=(Vector2::Arg v);
|
||||
void operator-=(Vector2::Arg v);
|
||||
void operator*=(scalar s);
|
||||
void operator*=(Vector2::Arg v);
|
||||
|
||||
friend bool operator==(Vector2::Arg a, Vector2::Arg b);
|
||||
friend bool operator!=(Vector2::Arg a, Vector2::Arg b);
|
||||
|
||||
private:
|
||||
scalar m_x, m_y;
|
||||
};
|
||||
|
||||
|
||||
class NVMATH_CLASS Vector3
|
||||
{
|
||||
public:
|
||||
typedef Vector3 const & Arg;
|
||||
|
||||
Vector3();
|
||||
explicit Vector3(zero_t);
|
||||
Vector3(scalar x, scalar y, scalar z);
|
||||
Vector3(Vector2::Arg v, scalar z);
|
||||
Vector3(Vector3::Arg v);
|
||||
|
||||
const Vector3 & operator=(Vector3::Arg v);
|
||||
|
||||
scalar x() const;
|
||||
scalar y() const;
|
||||
scalar z() const;
|
||||
|
||||
const Vector2 & xy() const;
|
||||
|
||||
// @@ temporary... should use an explicit method?
|
||||
const scalar * ptr() const;
|
||||
|
||||
void set(scalar x, scalar y, scalar z);
|
||||
|
||||
Vector3 operator-() const;
|
||||
void operator+=(Vector3::Arg v);
|
||||
void operator-=(Vector3::Arg v);
|
||||
void operator*=(scalar s);
|
||||
void operator*=(Vector3::Arg v);
|
||||
|
||||
friend bool operator==(Vector3::Arg a, Vector3::Arg b);
|
||||
friend bool operator!=(Vector3::Arg a, Vector3::Arg b);
|
||||
|
||||
private:
|
||||
scalar m_x, m_y, m_z;
|
||||
};
|
||||
|
||||
|
||||
class NVMATH_CLASS Vector4
|
||||
{
|
||||
public:
|
||||
typedef Vector4 const & Arg;
|
||||
|
||||
Vector4();
|
||||
explicit Vector4(zero_t);
|
||||
explicit Vector4(identity_t);
|
||||
Vector4(scalar x, scalar y, scalar z, scalar w);
|
||||
Vector4(Vector2::Arg v, scalar z, scalar w);
|
||||
Vector4(Vector3::Arg v, scalar w);
|
||||
Vector4(Vector4::Arg v);
|
||||
// Vector4(const Quaternion & v);
|
||||
|
||||
const Vector4 & operator=(Vector4::Arg v);
|
||||
|
||||
scalar x() const;
|
||||
scalar y() const;
|
||||
scalar z() const;
|
||||
scalar w() const;
|
||||
|
||||
const Vector2 & xy() const;
|
||||
const Vector3 & xyz() const;
|
||||
|
||||
void set(scalar x, scalar y, scalar z, scalar w);
|
||||
|
||||
Vector4 operator-() const;
|
||||
void operator+=(Vector4::Arg v);
|
||||
void operator-=(Vector4::Arg v);
|
||||
void operator*=(scalar s);
|
||||
void operator*=(Vector4::Arg v);
|
||||
|
||||
friend bool operator==(Vector4::Arg a, Vector4::Arg b);
|
||||
friend bool operator!=(Vector4::Arg a, Vector4::Arg b);
|
||||
|
||||
private:
|
||||
scalar m_x, m_y, m_z, m_w;
|
||||
};
|
||||
|
||||
|
||||
// Vector2
|
||||
|
||||
inline Vector2::Vector2() {}
|
||||
inline Vector2::Vector2(zero_t) : m_x(0.0f), m_y(0.0f) {}
|
||||
inline Vector2::Vector2(scalar x, scalar y) : m_x(x), m_y(y) {}
|
||||
inline Vector2::Vector2(Vector2::Arg v) : m_x(v.x()), m_y(v.y()) {}
|
||||
|
||||
inline const Vector2 & Vector2::operator=(Vector2::Arg v)
|
||||
{
|
||||
m_x = v.x();
|
||||
m_y = v.y();
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline scalar Vector2::x() const { return m_x; }
|
||||
inline scalar Vector2::y() const { return m_y; }
|
||||
|
||||
inline void Vector2::set(scalar x, scalar y)
|
||||
{
|
||||
m_x = x;
|
||||
m_y = y;
|
||||
}
|
||||
|
||||
inline Vector2 Vector2::operator-() const
|
||||
{
|
||||
return Vector2(-m_x, -m_y);
|
||||
}
|
||||
|
||||
inline void Vector2::operator+=(Vector2::Arg v)
|
||||
{
|
||||
m_x += v.m_x;
|
||||
m_y += v.m_y;
|
||||
}
|
||||
|
||||
inline void Vector2::operator-=(Vector2::Arg v)
|
||||
{
|
||||
m_x -= v.m_x;
|
||||
m_y -= v.m_y;
|
||||
}
|
||||
|
||||
inline void Vector2::operator*=(scalar s)
|
||||
{
|
||||
m_x *= s;
|
||||
m_y *= s;
|
||||
}
|
||||
|
||||
inline void Vector2::operator*=(Vector2::Arg v)
|
||||
{
|
||||
m_x *= v.m_x;
|
||||
m_y *= v.m_y;
|
||||
}
|
||||
|
||||
inline bool operator==(Vector2::Arg a, Vector2::Arg b)
|
||||
{
|
||||
return a.m_x == b.m_x && a.m_y == b.m_y;
|
||||
}
|
||||
inline bool operator!=(Vector2::Arg a, Vector2::Arg b)
|
||||
{
|
||||
return a.m_x != b.m_x || a.m_y != b.m_y;
|
||||
}
|
||||
|
||||
|
||||
// Vector3
|
||||
|
||||
inline Vector3::Vector3() {}
|
||||
inline Vector3::Vector3(zero_t) : m_x(0.0f), m_y(0.0f), m_z(0.0f) {}
|
||||
inline Vector3::Vector3(scalar x, scalar y, scalar z) : m_x(x), m_y(y), m_z(z) {}
|
||||
inline Vector3::Vector3(Vector2::Arg v, scalar z) : m_x(v.x()), m_y(v.y()), m_z(z) {}
|
||||
inline Vector3::Vector3(Vector3::Arg v) : m_x(v.x()), m_y(v.y()), m_z(v.z()) {}
|
||||
|
||||
inline const Vector3 & Vector3::operator=(Vector3::Arg v)
|
||||
{
|
||||
m_x = v.m_x;
|
||||
m_y = v.m_y;
|
||||
m_z = v.m_z;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline scalar Vector3::x() const { return m_x; }
|
||||
inline scalar Vector3::y() const { return m_y; }
|
||||
inline scalar Vector3::z() const { return m_z; }
|
||||
|
||||
inline const Vector2 & Vector3::xy() const
|
||||
{
|
||||
return *(Vector2 *)this;
|
||||
}
|
||||
|
||||
inline const scalar * Vector3::ptr() const
|
||||
{
|
||||
return &m_x;
|
||||
}
|
||||
|
||||
|
||||
inline void Vector3::set(scalar x, scalar y, scalar z)
|
||||
{
|
||||
m_x = x;
|
||||
m_y = y;
|
||||
m_z = z;
|
||||
}
|
||||
|
||||
inline Vector3 Vector3::operator-() const
|
||||
{
|
||||
return Vector3(-m_x, -m_y, -m_z);
|
||||
}
|
||||
|
||||
inline void Vector3::operator+=(Vector3::Arg v)
|
||||
{
|
||||
m_x += v.m_x;
|
||||
m_y += v.m_y;
|
||||
m_z += v.m_z;
|
||||
}
|
||||
|
||||
inline void Vector3::operator-=(Vector3::Arg v)
|
||||
{
|
||||
m_x -= v.m_x;
|
||||
m_y -= v.m_y;
|
||||
m_z -= v.m_z;
|
||||
}
|
||||
|
||||
inline void Vector3::operator*=(scalar s)
|
||||
{
|
||||
m_x *= s;
|
||||
m_y *= s;
|
||||
m_z *= s;
|
||||
}
|
||||
|
||||
inline void Vector3::operator*=(Vector3::Arg v)
|
||||
{
|
||||
m_x *= v.m_x;
|
||||
m_y *= v.m_y;
|
||||
m_z *= v.m_z;
|
||||
}
|
||||
|
||||
inline bool operator==(Vector3::Arg a, Vector3::Arg b)
|
||||
{
|
||||
return a.m_x == b.m_x && a.m_y == b.m_y && a.m_z == b.m_z;
|
||||
}
|
||||
inline bool operator!=(Vector3::Arg a, Vector3::Arg b)
|
||||
{
|
||||
return a.m_x != b.m_x || a.m_y != b.m_y || a.m_z != b.m_z;
|
||||
}
|
||||
|
||||
|
||||
// Vector4
|
||||
|
||||
inline Vector4::Vector4() {}
|
||||
inline Vector4::Vector4(zero_t) : m_x(0.0f), m_y(0.0f), m_z(0.0f), m_w(0.0f) {}
|
||||
inline Vector4::Vector4(identity_t) : m_x(0.0f), m_y(0.0f), m_z(0.0f), m_w(1.0f) {}
|
||||
inline Vector4::Vector4(scalar x, scalar y, scalar z, scalar w) : m_x(x), m_y(y), m_z(z), m_w(w) {}
|
||||
inline Vector4::Vector4(Vector2::Arg v, scalar z, scalar w) : m_x(v.x()), m_y(v.y()), m_z(z), m_w(w) {}
|
||||
inline Vector4::Vector4(Vector3::Arg v, scalar w) : m_x(v.x()), m_y(v.y()), m_z(v.z()), m_w(w) {}
|
||||
inline Vector4::Vector4(Vector4::Arg v) : m_x(v.x()), m_y(v.y()), m_z(v.z()), m_w(v.w()) {}
|
||||
|
||||
inline const Vector4 & Vector4::operator=(const Vector4 & v)
|
||||
{
|
||||
m_x = v.m_x;
|
||||
m_y = v.m_y;
|
||||
m_z = v.m_z;
|
||||
m_w = v.m_w;
|
||||
return *this;
|
||||
}
|
||||
|
||||
inline scalar Vector4::x() const { return m_x; }
|
||||
inline scalar Vector4::y() const { return m_y; }
|
||||
inline scalar Vector4::z() const { return m_z; }
|
||||
inline scalar Vector4::w() const { return m_w; }
|
||||
|
||||
inline const Vector2 & Vector4::xy() const
|
||||
{
|
||||
return *(Vector2 *)this;
|
||||
}
|
||||
|
||||
inline const Vector3 & Vector4::xyz() const
|
||||
{
|
||||
return *(Vector3 *)this;
|
||||
}
|
||||
|
||||
inline void Vector4::set(scalar x, scalar y, scalar z, scalar w)
|
||||
{
|
||||
m_x = x;
|
||||
m_y = y;
|
||||
m_z = z;
|
||||
m_w = w;
|
||||
}
|
||||
|
||||
inline Vector4 Vector4::operator-() const
|
||||
{
|
||||
return Vector4(-m_x, -m_y, -m_z, -m_w);
|
||||
}
|
||||
|
||||
inline void Vector4::operator+=(Vector4::Arg v)
|
||||
{
|
||||
m_x += v.m_x;
|
||||
m_y += v.m_y;
|
||||
m_z += v.m_z;
|
||||
m_w += v.m_w;
|
||||
}
|
||||
|
||||
inline void Vector4::operator-=(Vector4::Arg v)
|
||||
{
|
||||
m_x -= v.m_x;
|
||||
m_y -= v.m_y;
|
||||
m_z -= v.m_z;
|
||||
m_w -= v.m_w;
|
||||
}
|
||||
|
||||
inline void Vector4::operator*=(scalar s)
|
||||
{
|
||||
m_x *= s;
|
||||
m_y *= s;
|
||||
m_z *= s;
|
||||
m_w *= s;
|
||||
}
|
||||
|
||||
inline void Vector4::operator*=(Vector4::Arg v)
|
||||
{
|
||||
m_x *= v.m_x;
|
||||
m_y *= v.m_y;
|
||||
m_z *= v.m_z;
|
||||
m_w *= v.m_w;
|
||||
}
|
||||
|
||||
inline bool operator==(Vector4::Arg a, Vector4::Arg b)
|
||||
{
|
||||
return a.m_x == b.m_x && a.m_y == b.m_y && a.m_z == b.m_z && a.m_w == b.m_w;
|
||||
}
|
||||
inline bool operator!=(Vector4::Arg a, Vector4::Arg b)
|
||||
{
|
||||
return a.m_x != b.m_x || a.m_y != b.m_y || a.m_z != b.m_z || a.m_w != b.m_w;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Functions
|
||||
|
||||
|
||||
// Vector2
|
||||
|
||||
inline Vector2 add(Vector2::Arg a, Vector2::Arg b)
|
||||
{
|
||||
return Vector2(a.x() + b.x(), a.y() + b.y());
|
||||
}
|
||||
inline Vector2 operator+(Vector2::Arg a, Vector2::Arg b)
|
||||
{
|
||||
return add(a, b);
|
||||
}
|
||||
|
||||
inline Vector2 sub(Vector2::Arg a, Vector2::Arg b)
|
||||
{
|
||||
return Vector2(a.x() - b.x(), a.y() - b.y());
|
||||
}
|
||||
inline Vector2 operator-(Vector2::Arg a, Vector2::Arg b)
|
||||
{
|
||||
return sub(a, b);
|
||||
}
|
||||
|
||||
inline Vector2 scale(Vector2::Arg v, scalar s)
|
||||
{
|
||||
return Vector2(v.x() * s, v.y() * s);
|
||||
}
|
||||
|
||||
inline Vector2 scale(Vector2::Arg v, Vector2::Arg s)
|
||||
{
|
||||
return Vector2(v.x() * s.x(), v.y() * s.y());
|
||||
}
|
||||
|
||||
inline Vector2 operator*(Vector2::Arg v, scalar s)
|
||||
{
|
||||
return scale(v, s);
|
||||
}
|
||||
|
||||
inline Vector2 operator*(scalar s, Vector2::Arg v)
|
||||
{
|
||||
return scale(v, s);
|
||||
}
|
||||
|
||||
inline scalar dot(Vector2::Arg a, Vector2::Arg b)
|
||||
{
|
||||
return a.x() * b.x() + a.y() * b.y();
|
||||
}
|
||||
|
||||
inline scalar length_squared(Vector2::Arg v)
|
||||
{
|
||||
return v.x() * v.x() + v.y() * v.y();
|
||||
}
|
||||
|
||||
inline scalar length(Vector2::Arg v)
|
||||
{
|
||||
return sqrtf(length_squared(v));
|
||||
}
|
||||
|
||||
inline Vector2 min(Vector2::Arg a, Vector2::Arg b)
|
||||
{
|
||||
return Vector2(min(a.x(), b.x()), min(a.y(), b.y()));
|
||||
}
|
||||
|
||||
inline Vector2 max(Vector2::Arg a, Vector2::Arg b)
|
||||
{
|
||||
return Vector2(max(a.x(), b.x()), max(a.y(), b.y()));
|
||||
}
|
||||
|
||||
inline bool isValid(Vector2::Arg v)
|
||||
{
|
||||
return isFinite(v.x()) && isFinite(v.y());
|
||||
}
|
||||
|
||||
|
||||
// Vector3
|
||||
|
||||
inline Vector3 add(Vector3::Arg a, Vector3::Arg b)
|
||||
{
|
||||
return Vector3(a.x() + b.x(), a.y() + b.y(), a.z() + b.z());
|
||||
}
|
||||
inline Vector3 operator+(Vector3::Arg a, Vector3::Arg b)
|
||||
{
|
||||
return add(a, b);
|
||||
}
|
||||
|
||||
inline Vector3 sub(Vector3::Arg a, Vector3::Arg b)
|
||||
{
|
||||
return Vector3(a.x() - b.x(), a.y() - b.y(), a.z() - b.z());
|
||||
}
|
||||
inline Vector3 operator-(Vector3::Arg a, Vector3::Arg b)
|
||||
{
|
||||
return sub(a, b);
|
||||
}
|
||||
|
||||
inline Vector3 cross(Vector3::Arg a, Vector3::Arg b)
|
||||
{
|
||||
return Vector3(a.y() * b.z() - a.z() * b.y(), a.z() * b.x() - a.x() * b.z(), a.x() * b.y() - a.y() * b.x());
|
||||
}
|
||||
|
||||
inline Vector3 scale(Vector3::Arg v, scalar s)
|
||||
{
|
||||
return Vector3(v.x() * s, v.y() * s, v.z() * s);
|
||||
}
|
||||
|
||||
inline Vector3 scale(Vector3::Arg v, Vector3::Arg s)
|
||||
{
|
||||
return Vector3(v.x() * s.x(), v.y() * s.y(), v.z() * s.z());
|
||||
}
|
||||
|
||||
inline Vector3 operator*(Vector3::Arg v, scalar s)
|
||||
{
|
||||
return scale(v, s);
|
||||
}
|
||||
|
||||
inline Vector3 operator*(scalar s, Vector3::Arg v)
|
||||
{
|
||||
return scale(v, s);
|
||||
}
|
||||
|
||||
inline Vector3 operator/(Vector3::Arg v, scalar s)
|
||||
{
|
||||
return scale(v, 1.0f/s);
|
||||
}
|
||||
|
||||
inline Vector3 add_scaled(Vector3::Arg a, Vector3::Arg b, scalar s)
|
||||
{
|
||||
return Vector3(a.x() + b.x() * s, a.y() + b.y() * s, a.z() + b.z() * s);
|
||||
}
|
||||
|
||||
inline Vector3 lerp(Vector3::Arg v1, Vector3::Arg v2, scalar t)
|
||||
{
|
||||
const scalar s = 1.0f - t;
|
||||
return Vector3(v1.x() * s + t * v2.x(), v1.y() * s + t * v2.y(), v1.z() * s + t * v2.z());
|
||||
}
|
||||
|
||||
inline scalar dot(Vector3::Arg a, Vector3::Arg b)
|
||||
{
|
||||
return a.x() * b.x() + a.y() * b.y() + a.z() * b.z();
|
||||
}
|
||||
|
||||
inline scalar length_squared(Vector3::Arg v)
|
||||
{
|
||||
return v.x() * v.x() + v.y() * v.y() + v.z() * v.z();
|
||||
}
|
||||
|
||||
inline scalar length(Vector3::Arg v)
|
||||
{
|
||||
return sqrtf(length_squared(v));
|
||||
}
|
||||
|
||||
inline bool isNormalized(Vector3::Arg v, float epsilon = NV_NORMAL_EPSILON)
|
||||
{
|
||||
return equal(length(v), 1, epsilon);
|
||||
}
|
||||
|
||||
inline Vector3 normalize(Vector3::Arg v, float epsilon = NV_EPSILON)
|
||||
{
|
||||
float l = length(v);
|
||||
nvDebugCheck(!isZero(l, epsilon));
|
||||
Vector3 n = scale(v, 1.0f / l);
|
||||
nvDebugCheck(isNormalized(n));
|
||||
return n;
|
||||
}
|
||||
|
||||
inline Vector3 normalizeSafe(Vector3::Arg v, Vector3::Arg fallback, float epsilon = NV_EPSILON)
|
||||
{
|
||||
float l = length(v);
|
||||
if (isZero(l, epsilon)) {
|
||||
return fallback;
|
||||
}
|
||||
return scale(v, 1.0f / l);
|
||||
}
|
||||
|
||||
inline Vector3 min(Vector3::Arg a, Vector3::Arg b)
|
||||
{
|
||||
return Vector3(min(a.x(), b.x()), min(a.y(), b.y()), min(a.z(), b.z()));
|
||||
}
|
||||
|
||||
inline Vector3 max(Vector3::Arg a, Vector3::Arg b)
|
||||
{
|
||||
return Vector3(max(a.x(), b.x()), max(a.y(), b.y()), max(a.z(), b.z()));
|
||||
}
|
||||
|
||||
inline bool isValid(Vector3::Arg v)
|
||||
{
|
||||
return isFinite(v.x()) && isFinite(v.y()) && isFinite(v.z());
|
||||
}
|
||||
|
||||
/*
|
||||
Vector3 transform(Quaternion, vector3);
|
||||
Vector3 transform_point(matrix34, vector3);
|
||||
Vector3 transform_vector(matrix34, vector3);
|
||||
Vector3 transform_point(matrix44, vector3);
|
||||
Vector3 transform_vector(matrix44, vector3);
|
||||
*/
|
||||
|
||||
// Vector4
|
||||
|
||||
inline Vector4 add(Vector4::Arg a, Vector4::Arg b)
|
||||
{
|
||||
return Vector4(a.x() + b.x(), a.y() + b.y(), a.z() + b.z(), a.w() + b.w());
|
||||
}
|
||||
inline Vector4 operator+(Vector4::Arg a, Vector4::Arg b)
|
||||
{
|
||||
return add(a, b);
|
||||
}
|
||||
|
||||
inline Vector4 sub(Vector4::Arg a, Vector4::Arg b)
|
||||
{
|
||||
return Vector4(a.x() - b.x(), a.y() - b.y(), a.z() - b.z(), a.w() - b.w());
|
||||
}
|
||||
inline Vector4 operator-(Vector4::Arg a, Vector4::Arg b)
|
||||
{
|
||||
return sub(a, b);
|
||||
}
|
||||
|
||||
inline Vector4 scale(Vector4::Arg v, scalar s)
|
||||
{
|
||||
return Vector4(v.x() * s, v.y() * s, v.z() * s, v.w() * s);
|
||||
}
|
||||
|
||||
inline Vector4 scale(Vector4::Arg v, Vector4::Arg s)
|
||||
{
|
||||
return Vector4(v.x() * s.x(), v.y() * s.y(), v.z() * s.z(), v.w() * s.w());
|
||||
}
|
||||
|
||||
inline Vector4 operator*(Vector4::Arg v, scalar s)
|
||||
{
|
||||
return scale(v, s);
|
||||
}
|
||||
|
||||
inline Vector4 operator*(scalar s, Vector4::Arg v)
|
||||
{
|
||||
return scale(v, s);
|
||||
}
|
||||
|
||||
inline Vector4 operator/(Vector4::Arg v, scalar s)
|
||||
{
|
||||
return scale(v, 1.0f/s);
|
||||
}
|
||||
|
||||
inline Vector4 add_scaled(Vector4::Arg a, Vector4::Arg b, scalar s)
|
||||
{
|
||||
return Vector4(a.x() + b.x() * s, a.y() + b.y() * s, a.z() + b.z() * s, a.w() + b.w() * s);
|
||||
}
|
||||
|
||||
inline scalar dot(Vector4::Arg a, Vector4::Arg b)
|
||||
{
|
||||
return a.x() * b.x() + a.y() * b.y() + a.z() * b.z() + a.w() * b.w();
|
||||
}
|
||||
|
||||
inline scalar length_squared(Vector4::Arg v)
|
||||
{
|
||||
return v.x() * v.x() + v.y() * v.y() + v.z() * v.z() + v.w() * v.w();
|
||||
}
|
||||
|
||||
inline scalar length(Vector4::Arg v)
|
||||
{
|
||||
return sqrtf(length_squared(v));
|
||||
}
|
||||
|
||||
inline bool isNormalized(Vector4::Arg v, float epsilon = NV_NORMAL_EPSILON)
|
||||
{
|
||||
return equal(length(v), 1, epsilon);
|
||||
}
|
||||
|
||||
inline Vector4 normalize(Vector4::Arg v, float epsilon = NV_EPSILON)
|
||||
{
|
||||
float l = length(v);
|
||||
nvDebugCheck(!isZero(l, epsilon));
|
||||
Vector4 n = scale(v, 1.0f / l);
|
||||
nvDebugCheck(isNormalized(n));
|
||||
return n;
|
||||
}
|
||||
|
||||
inline Vector4 normalizeSafe(Vector4::Arg v, Vector4::Arg fallback, float epsilon = NV_EPSILON)
|
||||
{
|
||||
float l = length(v);
|
||||
if (isZero(l, epsilon)) {
|
||||
return fallback;
|
||||
}
|
||||
return scale(v, 1.0f / l);
|
||||
}
|
||||
|
||||
inline Vector4 min(Vector4::Arg a, Vector4::Arg b)
|
||||
{
|
||||
return Vector4(min(a.x(), b.x()), min(a.y(), b.y()), min(a.z(), b.z()), min(a.w(), b.w()));
|
||||
}
|
||||
|
||||
inline Vector4 max(Vector4::Arg a, Vector4::Arg b)
|
||||
{
|
||||
return Vector4(max(a.x(), b.x()), max(a.y(), b.y()), max(a.z(), b.z()), max(a.w(), b.w()));
|
||||
}
|
||||
|
||||
inline bool isValid(Vector4::Arg v)
|
||||
{
|
||||
return isFinite(v.x()) && isFinite(v.y()) && isFinite(v.z()) && isFinite(v.w());
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
vector4 transform(matrix34, vector4);
|
||||
vector4 transform(matrix44, vector4);
|
||||
*/
|
||||
|
||||
/*
|
||||
Quaternion mul(Quaternion, Quaternion); // rotational composition
|
||||
Quaternion conjugate(Quaternion);
|
||||
Quaternion inverse(Quaternion);
|
||||
Quaternion axis_angle(const Vector3 & v, scalar s);
|
||||
*/
|
||||
|
||||
/*
|
||||
matrix34 add(matrix34, matrix34); // note: implicit '1' stays as '1'
|
||||
matrix34 operator+(matrix34, matrix34);
|
||||
matrix34 sub(matrix34, matrix34); // note: implicit '1' stays as '1'
|
||||
matrix34 operator-(matrix34, matrix34);
|
||||
matrix34 mul(matrix34, matrix34);
|
||||
matrix34 operator*(matrix34, matrix34);
|
||||
matrix34 mul(matrix34, quaternion4); // rotation multiplication
|
||||
matrix34 operator*(matrix34, quaternion4); // rotation multiplication
|
||||
matrix34 translation(vector3);
|
||||
matrix34 rotation(quaternion4);
|
||||
matrix34 rotation(vector3, scalar); // axis/angle
|
||||
|
||||
matrix44 add(matrix44, matrix44);
|
||||
matrix44 operator+(matrix44, matrix44);
|
||||
matrix44 sub(matrix44, matrix44);
|
||||
matrix44 operator-(matrix44, matrix44);
|
||||
matrix44 mul(matrix44, matrix44);
|
||||
matrix44 operator*(matrix44, matrix44);
|
||||
matrix44 mul(matrix44, quaternion4); // rotation multiplication
|
||||
matrix44 operator*(matrix44, quaternion4); // rotation multiplication
|
||||
matrix44 invert(matrix34);
|
||||
matrix44 invert(matrix44);
|
||||
matrix44 transpose(matrix34);
|
||||
matrix44 transpose(matrix44);
|
||||
*/
|
||||
|
||||
} // nv namespace
|
||||
|
||||
#endif // NV_MATH_VECTOR_H
|
||||
141
src/nvmath/nvmath.h
Normal file
141
src/nvmath/nvmath.h
Normal file
@ -0,0 +1,141 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_H
|
||||
#define NV_MATH_H
|
||||
|
||||
#include <nvcore/nvcore.h>
|
||||
#include <nvcore/Debug.h>
|
||||
|
||||
#include <math.h>
|
||||
|
||||
// Function linkage
|
||||
#if NVMATH_SHARED
|
||||
#ifdef NVMATH_EXPORTS
|
||||
#define NVMATH_API DLL_EXPORT
|
||||
#define NVMATH_CLASS DLL_EXPORT_CLASS
|
||||
#else
|
||||
#define NVMATH_API DLL_IMPORT
|
||||
#define NVMATH_CLASS DLL_IMPORT
|
||||
#endif
|
||||
#else // NVMATH_SHARED
|
||||
#define NVMATH_API
|
||||
#define NVMATH_CLASS
|
||||
#endif // NVMATH_SHARED
|
||||
|
||||
#ifndef PI
|
||||
#define PI float(3.1415926535897932384626433833)
|
||||
#endif
|
||||
|
||||
#define NV_EPSILON (0.0001f)
|
||||
#define NV_NORMAL_EPSILON (0.001f)
|
||||
|
||||
/*
|
||||
#define SQ(r) ((r)*(r))
|
||||
|
||||
#define SIGN_BITMASK 0x80000000
|
||||
|
||||
/// Integer representation of a floating-point value.
|
||||
#define IR(x) ((uint32 &)(x))
|
||||
|
||||
/// Absolute integer representation of a floating-point value
|
||||
#define AIR(x) (IR(x) & 0x7fffffff)
|
||||
|
||||
/// Floating-point representation of an integer value.
|
||||
#define FR(x) ((float&)(x))
|
||||
|
||||
/// Integer-based comparison of a floating point value.
|
||||
/// Don't use it blindly, it can be faster or slower than the FPU comparison, depends on the context.
|
||||
#define IS_NEGATIVE_FLOAT(x) (IR(x)&SIGN_BITMASK)
|
||||
*/
|
||||
|
||||
inline float sqrt_assert(const float f)
|
||||
{
|
||||
nvDebugCheck(f >= 0.0f);
|
||||
return sqrtf(f);
|
||||
}
|
||||
|
||||
inline float acos_assert(const float f)
|
||||
{
|
||||
nvDebugCheck(f >= -1.0f && f <= 1.0f);
|
||||
return acosf(f);
|
||||
}
|
||||
|
||||
inline float asin_assert(const float f)
|
||||
{
|
||||
nvDebugCheck(f >= -1.0f && f <= 1.0f);
|
||||
return asinf(f);
|
||||
}
|
||||
|
||||
// Replace default functions with asserting ones.
|
||||
#define sqrt sqrt_assert
|
||||
#define sqrtf sqrt_assert
|
||||
#define acos acos_assert
|
||||
#define acosf acos_assert
|
||||
#define asin asin_assert
|
||||
#define asinf asin_assert
|
||||
|
||||
#if NV_OS_WIN32
|
||||
#include <float.h>
|
||||
#endif
|
||||
|
||||
namespace nv
|
||||
{
|
||||
inline float toRadian(float degree) { return degree * (PI / 180.0f); }
|
||||
inline float toDegree(float radian) { return radian * (180.0f / PI); }
|
||||
|
||||
inline bool equal(const float f0, const float f1, const float epsilon = NV_EPSILON)
|
||||
{
|
||||
return fabs(f0-f1) <= epsilon;
|
||||
}
|
||||
|
||||
inline bool isZero(const float f, const float epsilon = NV_EPSILON)
|
||||
{
|
||||
return fabs(f) <= epsilon;
|
||||
}
|
||||
|
||||
inline bool isFinite(const float f)
|
||||
{
|
||||
#if NV_OS_WIN32
|
||||
return _finite(f) != 0;
|
||||
#elif NV_OS_DARWIN
|
||||
return isfinite(f);
|
||||
#elif NV_OS_LINUX
|
||||
return finitef(f);
|
||||
#else
|
||||
# error "isFinite not supported"
|
||||
#endif
|
||||
//return std::isfinite (f);
|
||||
//return finite (f);
|
||||
}
|
||||
|
||||
inline bool isNan(const float f)
|
||||
{
|
||||
#if NV_OS_WIN32
|
||||
return _isnan(f) != 0;
|
||||
#elif NV_OS_DARWIN
|
||||
return isnan(f);
|
||||
#elif NV_OS_LINUX
|
||||
return isnanf(f);
|
||||
#else
|
||||
# error "isNan not supported"
|
||||
#endif
|
||||
}
|
||||
|
||||
inline uint log2(uint i)
|
||||
{
|
||||
uint value = 0;
|
||||
while( i >>= 1 ) {
|
||||
value++;
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
inline float lerp(float f0, float f1, float t)
|
||||
{
|
||||
const float s = 1.0f - t;
|
||||
return f0 * s + f1 * t;
|
||||
}
|
||||
|
||||
} // nv
|
||||
|
||||
#endif // NV_MATH_H
|
||||
Reference in New Issue
Block a user