From 98b2377a114da61ba0e65149c9b074ef22abda91 Mon Sep 17 00:00:00 2001 From: castano Date: Tue, 6 Nov 2007 10:14:57 +0000 Subject: [PATCH] Merge internal branch. --- src/nvcore/CMakeLists.txt | 1 + src/nvcore/Containers.h | 14 +- src/nvcore/StdStream.h | 7 +- src/nvcore/TextWriter.cpp | 45 + src/nvcore/TextWriter.h | 45 +- src/nvmath/Basis.cpp | 346 +++---- src/nvmath/Basis.h | 156 +-- src/nvmath/Matrix.h | 1975 +++++++++++++++++++------------------ src/nvmath/Quaternion.h | 256 ++--- src/nvmath/TriBox.cpp | 18 +- src/nvmath/Triangle.cpp | 19 +- src/nvmath/Triangle.h | 164 +-- 12 files changed, 1532 insertions(+), 1514 deletions(-) create mode 100644 src/nvcore/TextWriter.cpp diff --git a/src/nvcore/CMakeLists.txt b/src/nvcore/CMakeLists.txt index a437ed2..f654e38 100644 --- a/src/nvcore/CMakeLists.txt +++ b/src/nvcore/CMakeLists.txt @@ -17,6 +17,7 @@ SET(CORE_SRCS TextReader.h TextReader.cpp TextWriter.h + TextWriter.cpp Tokenizer.h Tokenizer.cpp Radix.h diff --git a/src/nvcore/Containers.h b/src/nvcore/Containers.h index 329b110..cb5a655 100644 --- a/src/nvcore/Containers.h +++ b/src/nvcore/Containers.h @@ -23,12 +23,6 @@ Do not use memmove in insert & remove, use copy ctors instead. #include // memmove #include // for placement new -#ifndef USE_TU_CONTAINERS -#define USE_TU_CONTAINERS 1 -#endif - - -#if USE_TU_CONTAINERS #if NV_CC_GNUC // If typeof is available: @@ -57,7 +51,7 @@ struct PseudoIndexWrapper { return *reinterpret_cast(memory); } - uint8 memory[8]; // Increase the size if we have bigger enumerators. + uint8 memory[4]; // Increase the size if we have bigger enumerators. }; #define NV_FOREACH(i, container) \ @@ -70,7 +64,6 @@ struct PseudoIndexWrapper { # define foreach NV_FOREACH #endif -#endif // USE_TU_CONTAINERS namespace nv @@ -193,7 +186,6 @@ namespace nv virtual T current(); }; -#if USE_TU_CONTAINERS /** * Replacement for std::vector that is easier to debug and provides @@ -1060,8 +1052,8 @@ namespace nv }; -#endif // USE_TU_CONTAINERS - + + } // nv namespace #endif // NV_CORE_CONTAINER_H diff --git a/src/nvcore/StdStream.h b/src/nvcore/StdStream.h index a2384c1..58f1f65 100644 --- a/src/nvcore/StdStream.h +++ b/src/nvcore/StdStream.h @@ -314,7 +314,12 @@ public: { return m_s->isError(); } - + + virtual void clearError() + { + m_s->clearError(); + } + virtual bool isAtEnd() const { return m_s->isAtEnd(); diff --git a/src/nvcore/TextWriter.cpp b/src/nvcore/TextWriter.cpp new file mode 100644 index 0000000..b65a862 --- /dev/null +++ b/src/nvcore/TextWriter.cpp @@ -0,0 +1,45 @@ +// This code is in the public domain -- castanyo@yahoo.es + +#include + +using namespace nv; + + +/// Constructor +TextWriter::TextWriter(Stream * s) : + s(s), + str(1024) +{ + nvCheck(s != NULL); + nvCheck(s->isSaving()); +} + +void TextWriter::writeString(const char * str) +{ + nvDebugCheck(s != NULL); + s->serialize(const_cast(str), strlen(str)); +} + +void TextWriter::writeString(const char * str, uint len) +{ + nvDebugCheck(s != NULL); + s->serialize(const_cast(str), len); +} + +void TextWriter::write(const char * format, ...) +{ + va_list arg; + va_start(arg,format); + str.format(format, arg); + writeString(str.str(), str.length()); + va_end(arg); +} + +void TextWriter::write(const char * format, va_list arg) +{ + va_list tmp; + va_copy(tmp, arg); + str.format(format, arg); + writeString(str.str(), str.length()); + va_end(tmp); +} diff --git a/src/nvcore/TextWriter.h b/src/nvcore/TextWriter.h index 708f2c9..155373c 100644 --- a/src/nvcore/TextWriter.h +++ b/src/nvcore/TextWriter.h @@ -7,9 +7,6 @@ #include #include -// @@ NOT IMPLEMENTED !!! - - namespace nv { @@ -18,16 +15,12 @@ namespace nv { public: - /// Ctor. - TextWriter(Stream * s) : s(s), str(1024) { - nvDebugCheck(s != NULL); - nvCheck(s->IsSaving()); - } - - void write( const char * str, uint len ); - void write( const char * format, ... ) __attribute__((format (printf, 2, 3))); - void write( const char * format, va_list arg ); + TextWriter(Stream * s); + void writeString(const char * str); + void writeString(const char * str, uint len); + void write(const char * format, ...) __attribute__((format (printf, 2, 3))); + void write(const char * format, va_list arg); private: @@ -38,7 +31,35 @@ namespace nv }; + + inline TextWriter & operator<<( TextWriter & tw, int i) + { + tw.write("%d", i); + return tw; + } + + inline TextWriter & operator<<( TextWriter & tw, uint i) + { + tw.write("%u", i); + return tw; + } + + inline TextWriter & operator<<( TextWriter & tw, float f) + { + tw.write("%f", f); + return tw; + } + + inline TextWriter & operator<<( TextWriter & tw, const char * str) + { + tw.writeString(str); + return tw; + } + } // nv namespace + + + #endif // NVCORE_TEXTWRITER_H diff --git a/src/nvmath/Basis.cpp b/src/nvmath/Basis.cpp index 167cc38..085e25b 100644 --- a/src/nvmath/Basis.cpp +++ b/src/nvmath/Basis.cpp @@ -1,173 +1,173 @@ -// This code is in the public domain -- castanyo@yahoo.es - -#include - -using namespace nv; - - -/// Normalize basis vectors. -void Basis::normalize(float epsilon /*= NV_EPSILON*/) -{ - normal = ::normalize(normal, epsilon); - tangent = ::normalize(tangent, epsilon); - bitangent = ::normalize(bitangent, epsilon); -} - - -/// Gram-Schmidt orthogonalization. -/// @note Works only if the vectors are close to orthogonal. -void Basis::orthonormalize(float epsilon /*= NV_EPSILON*/) -{ - // N' = |N| - // T' = |T - (N' dot T) N'| - // B' = |B - (N' dot B) N' - (T' dot B) T'| - - normal = ::normalize(normal, epsilon); - - tangent -= normal * dot(normal, tangent); - tangent = ::normalize(tangent, epsilon); - - bitangent -= normal * dot(normal, bitangent); - bitangent -= tangent * dot(tangent, bitangent); - bitangent = ::normalize(bitangent, epsilon); -} - - -/// Robust orthonormalization. -/// Returns an orthonormal basis even when the original is degenerate. -void Basis::robustOrthonormalize(float epsilon /*= NV_EPSILON*/) -{ - if (length(normal) < epsilon) - { - normal = cross(tangent, bitangent); - - if (length(normal) < epsilon) - { - tangent = Vector3(1, 0, 0); - bitangent = Vector3(0, 1, 0); - normal = Vector3(0, 0, 1); - return; - } - } - normal = ::normalize(normal, epsilon); - - tangent -= normal * dot(normal, tangent); - bitangent -= normal * dot(normal, bitangent); - - if (length(tangent) < epsilon) - { - if (length(bitangent) < epsilon) - { - buildFrameForDirection(normal); - } - else - { - tangent = cross(bitangent, normal); - nvCheck(isNormalized(tangent, epsilon)); - } - } - else - { - tangent = ::normalize(tangent, epsilon); - bitangent -= tangent * dot(tangent, bitangent); - - if (length(bitangent) < epsilon) - { - bitangent = cross(tangent, normal); - nvCheck(isNormalized(bitangent)); - } - else - { - tangent = ::normalize(tangent, epsilon); - } - } - - // Check vector lengths. - nvCheck(isNormalized(normal, epsilon)); - nvCheck(isNormalized(tangent, epsilon)); - nvCheck(isNormalized(bitangent, epsilon)); - - // Check vector angles. - nvCheck(equal(dot(normal, tangent), 0.0f, epsilon)); - nvCheck(equal(dot(normal, bitangent), 0.0f, epsilon)); - nvCheck(equal(dot(tangent, bitangent), 0.0f, epsilon)); - - // Check vector orientation. - const float det = dot(cross(normal, tangent), bitangent); - nvCheck(equal(det, 1.0f, epsilon) || equal(det, -1.0f, epsilon)); -} - - -/// Build an arbitrary frame for the given direction. -void Basis::buildFrameForDirection(Vector3::Arg d) -{ - nvCheck(isNormalized(d)); - normal = d; - - // Choose minimum axis. - if (fabsf(normal.x()) < fabsf(normal.y()) && fabsf(normal.x()) < fabsf(normal.z())) - { - tangent = Vector3(1, 0, 0); - } - else if (fabsf(normal.y()) < fabsf(normal.z())) - { - tangent = Vector3(0, 1, 0); - } - else - { - tangent = Vector3(0, 0, 1); - } - - // Ortogonalize - tangent -= normal * dot(normal, tangent); - tangent = ::normalize(tangent); - - bitangent = cross(normal, tangent); -} - - - -/* -/// Transform by this basis. (From this basis to object space). -Vector3 Basis::transform(Vector3::Arg v) const -{ - Vector3 o = tangent * v.x(); - o += bitangent * v.y(); - o += normal * v.z(); - return o; -} - -/// Transform by the transpose. (From object space to this basis). -Vector3 Basis::transformT(Vector3::Arg v) -{ - return Vector3(dot(tangent, v), dot(bitangent, v), dot(normal, v)); -} - -/// Transform by the inverse. (From object space to this basis). -/// @note Uses Kramer's rule so the inverse is not accurate if the basis is ill-conditioned. -Vector3 Basis::transformI(Vector3::Arg v) const -{ - const float det = determinant(); - nvCheck(!equalf(det, 0.0f)); - - const float idet = 1.0f / det; - - // Rows of the inverse matrix. - Vector3 r0, r1, r2; - r0.x = (bitangent.y() * normal.z() - bitangent.z() * normal.y()) * idet; - r0.y = -(bitangent.x() * normal.z() - bitangent.z() * normal.x()) * idet; - r0.z = (bitangent.x() * normal.y() - bitangent.y() * normal.x()) * idet; - - r1.x = -(tangent.y() * normal.z() - tangent.z() * normal.y()) * idet; - r1.y = (tangent.x() * normal.z() - tangent.z() * normal.x()) * idet; - r1.z = -(tangent.x() * normal.y() - tangent.y() * normal.x()) * idet; - - r2.x = (tangent.y() * bitangent.z() - tangent.z() * bitangent.y()) * idet; - r2.y = -(tangent.x() * bitangent.z() - tangent.z() * bitangent.x()) * idet; - r2.z = (tangent.x() * bitangent.y() - tangent.y() * bitangent.x()) * idet; - - return Vector3(dot(v, r0), dot(v, r1), dot(v, r2)); -} -*/ - - +// This code is in the public domain -- castanyo@yahoo.es + +#include + +using namespace nv; + + +/// Normalize basis vectors. +void Basis::normalize(float epsilon /*= NV_EPSILON*/) +{ + normal = ::normalize(normal, epsilon); + tangent = ::normalize(tangent, epsilon); + bitangent = ::normalize(bitangent, epsilon); +} + + +/// Gram-Schmidt orthogonalization. +/// @note Works only if the vectors are close to orthogonal. +void Basis::orthonormalize(float epsilon /*= NV_EPSILON*/) +{ + // N' = |N| + // T' = |T - (N' dot T) N'| + // B' = |B - (N' dot B) N' - (T' dot B) T'| + + normal = ::normalize(normal, epsilon); + + tangent -= normal * dot(normal, tangent); + tangent = ::normalize(tangent, epsilon); + + bitangent -= normal * dot(normal, bitangent); + bitangent -= tangent * dot(tangent, bitangent); + bitangent = ::normalize(bitangent, epsilon); +} + + +/// Robust orthonormalization. +/// Returns an orthonormal basis even when the original is degenerate. +void Basis::robustOrthonormalize(float epsilon /*= NV_EPSILON*/) +{ + if (length(normal) < epsilon) + { + normal = cross(tangent, bitangent); + + if (length(normal) < epsilon) + { + tangent = Vector3(1, 0, 0); + bitangent = Vector3(0, 1, 0); + normal = Vector3(0, 0, 1); + return; + } + } + normal = ::normalize(normal, epsilon); + + tangent -= normal * dot(normal, tangent); + bitangent -= normal * dot(normal, bitangent); + + if (length(tangent) < epsilon) + { + if (length(bitangent) < epsilon) + { + buildFrameForDirection(normal); + } + else + { + tangent = cross(bitangent, normal); + nvCheck(isNormalized(tangent, epsilon)); + } + } + else + { + tangent = ::normalize(tangent, epsilon); + bitangent -= tangent * dot(tangent, bitangent); + + if (length(bitangent) < epsilon) + { + bitangent = cross(tangent, normal); + nvCheck(isNormalized(bitangent)); + } + else + { + tangent = ::normalize(tangent, epsilon); + } + } + + // Check vector lengths. + nvCheck(isNormalized(normal, epsilon)); + nvCheck(isNormalized(tangent, epsilon)); + nvCheck(isNormalized(bitangent, epsilon)); + + // Check vector angles. + nvCheck(equal(dot(normal, tangent), 0.0f, epsilon)); + nvCheck(equal(dot(normal, bitangent), 0.0f, epsilon)); + nvCheck(equal(dot(tangent, bitangent), 0.0f, epsilon)); + + // Check vector orientation. + const float det = dot(cross(normal, tangent), bitangent); + nvCheck(equal(det, 1.0f, epsilon) || equal(det, -1.0f, epsilon)); +} + + +/// Build an arbitrary frame for the given direction. +void Basis::buildFrameForDirection(Vector3::Arg d) +{ + nvCheck(isNormalized(d)); + normal = d; + + // Choose minimum axis. + if (fabsf(normal.x()) < fabsf(normal.y()) && fabsf(normal.x()) < fabsf(normal.z())) + { + tangent = Vector3(1, 0, 0); + } + else if (fabsf(normal.y()) < fabsf(normal.z())) + { + tangent = Vector3(0, 1, 0); + } + else + { + tangent = Vector3(0, 0, 1); + } + + // Ortogonalize + tangent -= normal * dot(normal, tangent); + tangent = ::normalize(tangent); + + bitangent = cross(normal, tangent); +} + + + +/* +/// Transform by this basis. (From this basis to object space). +Vector3 Basis::transform(Vector3::Arg v) const +{ + Vector3 o = tangent * v.x(); + o += bitangent * v.y(); + o += normal * v.z(); + return o; +} + +/// Transform by the transpose. (From object space to this basis). +Vector3 Basis::transformT(Vector3::Arg v) +{ + return Vector3(dot(tangent, v), dot(bitangent, v), dot(normal, v)); +} + +/// Transform by the inverse. (From object space to this basis). +/// @note Uses Kramer's rule so the inverse is not accurate if the basis is ill-conditioned. +Vector3 Basis::transformI(Vector3::Arg v) const +{ + const float det = determinant(); + nvCheck(!equalf(det, 0.0f)); + + const float idet = 1.0f / det; + + // Rows of the inverse matrix. + Vector3 r0, r1, r2; + r0.x = (bitangent.y() * normal.z() - bitangent.z() * normal.y()) * idet; + r0.y = -(bitangent.x() * normal.z() - bitangent.z() * normal.x()) * idet; + r0.z = (bitangent.x() * normal.y() - bitangent.y() * normal.x()) * idet; + + r1.x = -(tangent.y() * normal.z() - tangent.z() * normal.y()) * idet; + r1.y = (tangent.x() * normal.z() - tangent.z() * normal.x()) * idet; + r1.z = -(tangent.x() * normal.y() - tangent.y() * normal.x()) * idet; + + r2.x = (tangent.y() * bitangent.z() - tangent.z() * bitangent.y()) * idet; + r2.y = -(tangent.x() * bitangent.z() - tangent.z() * bitangent.x()) * idet; + r2.z = (tangent.x() * bitangent.y() - tangent.y() * bitangent.x()) * idet; + + return Vector3(dot(v, r0), dot(v, r1), dot(v, r2)); +} +*/ + + diff --git a/src/nvmath/Basis.h b/src/nvmath/Basis.h index 90247be..7adde57 100644 --- a/src/nvmath/Basis.h +++ b/src/nvmath/Basis.h @@ -1,78 +1,78 @@ -// This code is in the public domain -- castanyo@yahoo.es - -#ifndef NV_MATH_BASIS_H -#define NV_MATH_BASIS_H - -#include -#include -#include - -namespace nv -{ - - /// Basis class to compute tangent space basis, ortogonalizations and to - /// transform vectors from one space to another. - struct Basis - { - /// Create a null basis. - Basis() : tangent(0, 0, 0), bitangent(0, 0, 0), normal(0, 0, 0) {} - - /// Create a basis given three vectors. - Basis(Vector3::Arg n, Vector3::Arg t, Vector3::Arg b) : tangent(t), bitangent(b), normal(n) {} - - /// Create a basis with the given tangent vectors and the handness. - Basis(Vector3::Arg n, Vector3::Arg t, float sign) - { - build(n, t, sign); - } - - NVMATH_API void normalize(float epsilon = NV_EPSILON); - NVMATH_API void orthonormalize(float epsilon = NV_EPSILON); - NVMATH_API void robustOrthonormalize(float epsilon = NV_EPSILON); - NVMATH_API void buildFrameForDirection(Vector3::Arg d); - - /// Calculate the determinant [ F G N ] to obtain the handness of the basis. - float handness() const - { - return determinant() > 0.0f ? 1.0f : -1.0f; - } - - /// Build a basis from 2 vectors and a handness flag. - void build(Vector3::Arg n, Vector3::Arg t, float sign) - { - normal = n; - tangent = t; - bitangent = sign * cross(t, n); - } - - /// Compute the determinant of this basis. - float determinant() const - { - return - tangent.x() * bitangent.y() * normal.z() - tangent.z() * bitangent.y() * normal.x() + - tangent.y() * bitangent.z() * normal.x() - tangent.y() * bitangent.x() * normal.z() + - tangent.z() * bitangent.x() * normal.y() - tangent.x() * bitangent.z() * normal.y(); - } - - /* - // Get transform matrix for this basis. - NVMATH_API Matrix matrix() const; - - // Transform by this basis. (From this basis to object space). - NVMATH_API Vector3 transform(Vector3::Arg v) const; - - // Transform by the transpose. (From object space to this basis). - NVMATH_API Vector3 transformT(Vector3::Arg v); - - // Transform by the inverse. (From object space to this basis). - NVMATH_API Vector3 transformI(Vector3::Arg v) const; - */ - - Vector3 tangent; - Vector3 bitangent; - Vector3 normal; - }; - -} // nv namespace - -#endif // NV_MATH_BASIS_H +// This code is in the public domain -- castanyo@yahoo.es + +#ifndef NV_MATH_BASIS_H +#define NV_MATH_BASIS_H + +#include +#include +#include + +namespace nv +{ + + /// Basis class to compute tangent space basis, ortogonalizations and to + /// transform vectors from one space to another. + struct Basis + { + /// Create a null basis. + Basis() : tangent(0, 0, 0), bitangent(0, 0, 0), normal(0, 0, 0) {} + + /// Create a basis given three vectors. + Basis(Vector3::Arg n, Vector3::Arg t, Vector3::Arg b) : tangent(t), bitangent(b), normal(n) {} + + /// Create a basis with the given tangent vectors and the handness. + Basis(Vector3::Arg n, Vector3::Arg t, float sign) + { + build(n, t, sign); + } + + NVMATH_API void normalize(float epsilon = NV_EPSILON); + NVMATH_API void orthonormalize(float epsilon = NV_EPSILON); + NVMATH_API void robustOrthonormalize(float epsilon = NV_EPSILON); + NVMATH_API void buildFrameForDirection(Vector3::Arg d); + + /// Calculate the determinant [ F G N ] to obtain the handness of the basis. + float handness() const + { + return determinant() > 0.0f ? 1.0f : -1.0f; + } + + /// Build a basis from 2 vectors and a handness flag. + void build(Vector3::Arg n, Vector3::Arg t, float sign) + { + normal = n; + tangent = t; + bitangent = sign * cross(t, n); + } + + /// Compute the determinant of this basis. + float determinant() const + { + return + tangent.x() * bitangent.y() * normal.z() - tangent.z() * bitangent.y() * normal.x() + + tangent.y() * bitangent.z() * normal.x() - tangent.y() * bitangent.x() * normal.z() + + tangent.z() * bitangent.x() * normal.y() - tangent.x() * bitangent.z() * normal.y(); + } + + /* + // Get transform matrix for this basis. + NVMATH_API Matrix matrix() const; + + // Transform by this basis. (From this basis to object space). + NVMATH_API Vector3 transform(Vector3::Arg v) const; + + // Transform by the transpose. (From object space to this basis). + NVMATH_API Vector3 transformT(Vector3::Arg v); + + // Transform by the inverse. (From object space to this basis). + NVMATH_API Vector3 transformI(Vector3::Arg v) const; + */ + + Vector3 tangent; + Vector3 bitangent; + Vector3 normal; + }; + +} // nv namespace + +#endif // NV_MATH_BASIS_H diff --git a/src/nvmath/Matrix.h b/src/nvmath/Matrix.h index ec41553..4064816 100644 --- a/src/nvmath/Matrix.h +++ b/src/nvmath/Matrix.h @@ -1,975 +1,1000 @@ -// This code is in the public domain -- castanyo@yahoo.es - -#ifndef NV_MATH_MATRIX_H -#define NV_MATH_MATRIX_H - -#include -#include - -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 data(uint idx) const; - scalar & data(uint idx); - 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); - scalar determinant() const; - - 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::data(uint idx) const -{ - nvDebugCheck(idx < 16); - return m_data[idx]; -} -inline scalar & Matrix::data(uint idx) -{ - nvDebugCheck(idx < 16); - return m_data[idx]; -} -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; - m_data[12] *= s; m_data[13] *= s; m_data[14] *= s; m_data[15] *= 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 scalar 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 Matrix::determinant() const -{ - return - m_data[3] * m_data[6] * m_data[ 9] * m_data[12] - m_data[2] * m_data[7] * m_data[ 9] * m_data[12] - m_data[3] * m_data[5] * m_data[10] * m_data[12] + m_data[1] * m_data[7] * m_data[10] * m_data[12] + - m_data[2] * m_data[5] * m_data[11] * m_data[12] - m_data[1] * m_data[6] * m_data[11] * m_data[12] - m_data[3] * m_data[6] * m_data[ 8] * m_data[13] + m_data[2] * m_data[7] * m_data[ 8] * m_data[13] + - m_data[3] * m_data[4] * m_data[10] * m_data[13] - m_data[0] * m_data[7] * m_data[10] * m_data[13] - m_data[2] * m_data[4] * m_data[11] * m_data[13] + m_data[0] * m_data[6] * m_data[11] * m_data[13] + - m_data[3] * m_data[5] * m_data[ 8] * m_data[14] - m_data[1] * m_data[7] * m_data[ 8] * m_data[14] - m_data[3] * m_data[4] * m_data[ 9] * m_data[14] + m_data[0] * m_data[7] * m_data[ 9] * m_data[14] + - m_data[1] * m_data[4] * m_data[11] * m_data[14] - m_data[0] * m_data[5] * m_data[11] * m_data[14] - m_data[2] * m_data[5] * m_data[ 8] * m_data[15] + m_data[1] * m_data[6] * m_data[ 8] * m_data[15] + - m_data[2] * m_data[4] * m_data[ 9] * m_data[15] - m_data[0] * m_data[6] * m_data[ 9] * m_data[15] - m_data[1] * m_data[4] * m_data[10] * m_data[15] + m_data[0] * m_data[5] * m_data[10] * m_data[15]; -} - -inline Matrix transpose(Matrix::Arg m) -{ - Matrix r; - for (int i = 0; i < 4; i++) - { - for (int j = 0; j < 4; i++) - { - r(i, j) = m(j, i); - } - } - return r; -} - -inline Matrix inverse(Matrix::Arg m) -{ - Matrix r; - r.data( 0) = m.data(6)*m.data(11)*m.data(13) - m.data(7)*m.data(10)*m.data(13) + m.data(7)*m.data(9)*m.data(14) - m.data(5)*m.data(11)*m.data(14) - m.data(6)*m.data(9)*m.data(15) + m.data(5)*m.data(10)*m.data(15); - r.data( 1) = m.data(3)*m.data(10)*m.data(13) - m.data(2)*m.data(11)*m.data(13) - m.data(3)*m.data(9)*m.data(14) + m.data(1)*m.data(11)*m.data(14) + m.data(2)*m.data(9)*m.data(15) - m.data(1)*m.data(10)*m.data(15); - r.data( 2) = m.data(2)*m.data( 7)*m.data(13) - m.data(3)*m.data( 6)*m.data(13) + m.data(3)*m.data(5)*m.data(14) - m.data(1)*m.data( 7)*m.data(14) - m.data(2)*m.data(5)*m.data(15) + m.data(1)*m.data( 6)*m.data(15); - r.data( 3) = m.data(3)*m.data( 6)*m.data( 9) - m.data(2)*m.data( 7)*m.data( 9) - m.data(3)*m.data(5)*m.data(10) + m.data(1)*m.data( 7)*m.data(10) + m.data(2)*m.data(5)*m.data(11) - m.data(1)*m.data( 6)*m.data(11); - r.data( 4) = m.data(7)*m.data(10)*m.data(12) - m.data(6)*m.data(11)*m.data(12) - m.data(7)*m.data(8)*m.data(14) + m.data(4)*m.data(11)*m.data(14) + m.data(6)*m.data(8)*m.data(15) - m.data(4)*m.data(10)*m.data(15); - r.data( 5) = m.data(2)*m.data(11)*m.data(12) - m.data(3)*m.data(10)*m.data(12) + m.data(3)*m.data(8)*m.data(14) - m.data(0)*m.data(11)*m.data(14) - m.data(2)*m.data(8)*m.data(15) + m.data(0)*m.data(10)*m.data(15); - r.data( 6) = m.data(3)*m.data( 6)*m.data(12) - m.data(2)*m.data( 7)*m.data(12) - m.data(3)*m.data(4)*m.data(14) + m.data(0)*m.data( 7)*m.data(14) + m.data(2)*m.data(4)*m.data(15) - m.data(0)*m.data( 6)*m.data(15); - r.data( 7) = m.data(2)*m.data( 7)*m.data( 8) - m.data(3)*m.data( 6)*m.data( 8) + m.data(3)*m.data(4)*m.data(10) - m.data(0)*m.data( 7)*m.data(10) - m.data(2)*m.data(4)*m.data(11) + m.data(0)*m.data( 6)*m.data(11); - r.data( 8) = m.data(5)*m.data(11)*m.data(12) - m.data(7)*m.data( 9)*m.data(12) + m.data(7)*m.data(8)*m.data(13) - m.data(4)*m.data(11)*m.data(13) - m.data(5)*m.data(8)*m.data(15) + m.data(4)*m.data( 9)*m.data(15); - r.data( 9) = m.data(3)*m.data( 9)*m.data(12) - m.data(1)*m.data(11)*m.data(12) - m.data(3)*m.data(8)*m.data(13) + m.data(0)*m.data(11)*m.data(13) + m.data(1)*m.data(8)*m.data(15) - m.data(0)*m.data( 9)*m.data(15); - r.data(10) = m.data(1)*m.data( 7)*m.data(12) - m.data(3)*m.data( 5)*m.data(12) + m.data(3)*m.data(4)*m.data(13) - m.data(0)*m.data( 7)*m.data(13) - m.data(1)*m.data(4)*m.data(15) + m.data(0)*m.data( 5)*m.data(15); - r.data(11) = m.data(3)*m.data( 5)*m.data( 8) - m.data(1)*m.data( 7)*m.data( 8) - m.data(3)*m.data(4)*m.data( 9) + m.data(0)*m.data( 7)*m.data( 9) + m.data(1)*m.data(4)*m.data(11) - m.data(0)*m.data( 5)*m.data(11); - r.data(12) = m.data(6)*m.data( 9)*m.data(12) - m.data(5)*m.data(10)*m.data(12) - m.data(6)*m.data(8)*m.data(13) + m.data(4)*m.data(10)*m.data(13) + m.data(5)*m.data(8)*m.data(14) - m.data(4)*m.data( 9)*m.data(14); - r.data(13) = m.data(1)*m.data(10)*m.data(12) - m.data(2)*m.data( 9)*m.data(12) + m.data(2)*m.data(8)*m.data(13) - m.data(0)*m.data(10)*m.data(13) - m.data(1)*m.data(8)*m.data(14) + m.data(0)*m.data( 9)*m.data(14); - r.data(14) = m.data(2)*m.data( 5)*m.data(12) - m.data(1)*m.data( 6)*m.data(12) - m.data(2)*m.data(4)*m.data(13) + m.data(0)*m.data( 6)*m.data(13) + m.data(1)*m.data(4)*m.data(14) - m.data(0)*m.data( 5)*m.data(14); - r.data(15) = m.data(1)*m.data( 6)*m.data( 8) - m.data(2)*m.data( 5)*m.data( 8) + m.data(2)*m.data(4)*m.data( 9) - m.data(0)*m.data( 6)*m.data( 9) - m.data(1)*m.data(4)*m.data(10) + m.data(0)*m.data( 5)*m.data(10); - r.scale(1.0f / m.determinant()); - return r; -} - -//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; // 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 +// This code is in the public domain -- castanyo@yahoo.es + +#ifndef NV_MATH_MATRIX_H +#define NV_MATH_MATRIX_H + +#include +#include + +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 data(uint idx) const; + scalar & data(uint idx); + 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); + scalar determinant() const; + + 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::data(uint idx) const +{ + nvDebugCheck(idx < 16); + return m_data[idx]; +} +inline scalar & Matrix::data(uint idx) +{ + nvDebugCheck(idx < 16); + return m_data[idx]; +} +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; + m_data[12] *= s; m_data[13] *= s; m_data[14] *= s; m_data[15] *= 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 scalar 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 Matrix::determinant() const +{ + return + m_data[3] * m_data[6] * m_data[ 9] * m_data[12] - m_data[2] * m_data[7] * m_data[ 9] * m_data[12] - m_data[3] * m_data[5] * m_data[10] * m_data[12] + m_data[1] * m_data[7] * m_data[10] * m_data[12] + + m_data[2] * m_data[5] * m_data[11] * m_data[12] - m_data[1] * m_data[6] * m_data[11] * m_data[12] - m_data[3] * m_data[6] * m_data[ 8] * m_data[13] + m_data[2] * m_data[7] * m_data[ 8] * m_data[13] + + m_data[3] * m_data[4] * m_data[10] * m_data[13] - m_data[0] * m_data[7] * m_data[10] * m_data[13] - m_data[2] * m_data[4] * m_data[11] * m_data[13] + m_data[0] * m_data[6] * m_data[11] * m_data[13] + + m_data[3] * m_data[5] * m_data[ 8] * m_data[14] - m_data[1] * m_data[7] * m_data[ 8] * m_data[14] - m_data[3] * m_data[4] * m_data[ 9] * m_data[14] + m_data[0] * m_data[7] * m_data[ 9] * m_data[14] + + m_data[1] * m_data[4] * m_data[11] * m_data[14] - m_data[0] * m_data[5] * m_data[11] * m_data[14] - m_data[2] * m_data[5] * m_data[ 8] * m_data[15] + m_data[1] * m_data[6] * m_data[ 8] * m_data[15] + + m_data[2] * m_data[4] * m_data[ 9] * m_data[15] - m_data[0] * m_data[6] * m_data[ 9] * m_data[15] - m_data[1] * m_data[4] * m_data[10] * m_data[15] + m_data[0] * m_data[5] * m_data[10] * m_data[15]; +} + +inline Matrix transpose(Matrix::Arg m) +{ + Matrix r; + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 4; i++) + { + r(i, j) = m(j, i); + } + } + return r; +} + +inline Matrix inverse(Matrix::Arg m) +{ + Matrix r; + r.data( 0) = m.data(6)*m.data(11)*m.data(13) - m.data(7)*m.data(10)*m.data(13) + m.data(7)*m.data(9)*m.data(14) - m.data(5)*m.data(11)*m.data(14) - m.data(6)*m.data(9)*m.data(15) + m.data(5)*m.data(10)*m.data(15); + r.data( 1) = m.data(3)*m.data(10)*m.data(13) - m.data(2)*m.data(11)*m.data(13) - m.data(3)*m.data(9)*m.data(14) + m.data(1)*m.data(11)*m.data(14) + m.data(2)*m.data(9)*m.data(15) - m.data(1)*m.data(10)*m.data(15); + r.data( 2) = m.data(2)*m.data( 7)*m.data(13) - m.data(3)*m.data( 6)*m.data(13) + m.data(3)*m.data(5)*m.data(14) - m.data(1)*m.data( 7)*m.data(14) - m.data(2)*m.data(5)*m.data(15) + m.data(1)*m.data( 6)*m.data(15); + r.data( 3) = m.data(3)*m.data( 6)*m.data( 9) - m.data(2)*m.data( 7)*m.data( 9) - m.data(3)*m.data(5)*m.data(10) + m.data(1)*m.data( 7)*m.data(10) + m.data(2)*m.data(5)*m.data(11) - m.data(1)*m.data( 6)*m.data(11); + r.data( 4) = m.data(7)*m.data(10)*m.data(12) - m.data(6)*m.data(11)*m.data(12) - m.data(7)*m.data(8)*m.data(14) + m.data(4)*m.data(11)*m.data(14) + m.data(6)*m.data(8)*m.data(15) - m.data(4)*m.data(10)*m.data(15); + r.data( 5) = m.data(2)*m.data(11)*m.data(12) - m.data(3)*m.data(10)*m.data(12) + m.data(3)*m.data(8)*m.data(14) - m.data(0)*m.data(11)*m.data(14) - m.data(2)*m.data(8)*m.data(15) + m.data(0)*m.data(10)*m.data(15); + r.data( 6) = m.data(3)*m.data( 6)*m.data(12) - m.data(2)*m.data( 7)*m.data(12) - m.data(3)*m.data(4)*m.data(14) + m.data(0)*m.data( 7)*m.data(14) + m.data(2)*m.data(4)*m.data(15) - m.data(0)*m.data( 6)*m.data(15); + r.data( 7) = m.data(2)*m.data( 7)*m.data( 8) - m.data(3)*m.data( 6)*m.data( 8) + m.data(3)*m.data(4)*m.data(10) - m.data(0)*m.data( 7)*m.data(10) - m.data(2)*m.data(4)*m.data(11) + m.data(0)*m.data( 6)*m.data(11); + r.data( 8) = m.data(5)*m.data(11)*m.data(12) - m.data(7)*m.data( 9)*m.data(12) + m.data(7)*m.data(8)*m.data(13) - m.data(4)*m.data(11)*m.data(13) - m.data(5)*m.data(8)*m.data(15) + m.data(4)*m.data( 9)*m.data(15); + r.data( 9) = m.data(3)*m.data( 9)*m.data(12) - m.data(1)*m.data(11)*m.data(12) - m.data(3)*m.data(8)*m.data(13) + m.data(0)*m.data(11)*m.data(13) + m.data(1)*m.data(8)*m.data(15) - m.data(0)*m.data( 9)*m.data(15); + r.data(10) = m.data(1)*m.data( 7)*m.data(12) - m.data(3)*m.data( 5)*m.data(12) + m.data(3)*m.data(4)*m.data(13) - m.data(0)*m.data( 7)*m.data(13) - m.data(1)*m.data(4)*m.data(15) + m.data(0)*m.data( 5)*m.data(15); + r.data(11) = m.data(3)*m.data( 5)*m.data( 8) - m.data(1)*m.data( 7)*m.data( 8) - m.data(3)*m.data(4)*m.data( 9) + m.data(0)*m.data( 7)*m.data( 9) + m.data(1)*m.data(4)*m.data(11) - m.data(0)*m.data( 5)*m.data(11); + r.data(12) = m.data(6)*m.data( 9)*m.data(12) - m.data(5)*m.data(10)*m.data(12) - m.data(6)*m.data(8)*m.data(13) + m.data(4)*m.data(10)*m.data(13) + m.data(5)*m.data(8)*m.data(14) - m.data(4)*m.data( 9)*m.data(14); + r.data(13) = m.data(1)*m.data(10)*m.data(12) - m.data(2)*m.data( 9)*m.data(12) + m.data(2)*m.data(8)*m.data(13) - m.data(0)*m.data(10)*m.data(13) - m.data(1)*m.data(8)*m.data(14) + m.data(0)*m.data( 9)*m.data(14); + r.data(14) = m.data(2)*m.data( 5)*m.data(12) - m.data(1)*m.data( 6)*m.data(12) - m.data(2)*m.data(4)*m.data(13) + m.data(0)*m.data( 6)*m.data(13) + m.data(1)*m.data(4)*m.data(14) - m.data(0)*m.data( 5)*m.data(14); + r.data(15) = m.data(1)*m.data( 6)*m.data( 8) - m.data(2)*m.data( 5)*m.data( 8) + m.data(2)*m.data(4)*m.data( 9) - m.data(0)*m.data( 6)*m.data( 9) - m.data(1)*m.data(4)*m.data(10) + m.data(0)*m.data( 5)*m.data(10); + r.scale(1.0f / m.determinant()); + return r; +} + +inline Matrix isometryInverse(Matrix::Arg m) +{ + Matrix r(identity); + + // transposed 3x3 upper left matrix + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + r(i, j) = m(j, i); + } + } + + // translate by the negative offsets + r.translate(-Vector3(m.data(12), m.data(13), m.data(14))); + + return r; +} + +//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)); +} + +inline Matrix mul(Matrix::Arg a, Matrix::Arg b) +{ + // @@ Is this the right order? mul(a, b) = b * a + Matrix m = a; + m.apply(b); + return m; +} + +} // 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; // 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 diff --git a/src/nvmath/Quaternion.h b/src/nvmath/Quaternion.h index 96ebf0e..b5007cc 100644 --- a/src/nvmath/Quaternion.h +++ b/src/nvmath/Quaternion.h @@ -1,128 +1,128 @@ -// This code is in the public domain -- castanyo@yahoo.es - -#ifndef NV_MATH_QUATERNION_H -#define NV_MATH_QUATERNION_H - -#include -#include - -namespace nv -{ - - class NVMATH_CLASS Quaternion - { - public: - typedef Quaternion const & Arg; - - Quaternion(); - explicit Quaternion(zero_t); - Quaternion(float x, float y, float z, float w); - Quaternion(Vector4::Arg v); - - const Quaternion & operator=(Quaternion::Arg v); - - scalar x() const; - scalar y() const; - scalar z() const; - scalar w() const; - - const Vector4 & asVector() const; - Vector4 & asVector(); - - private: - Vector4 q; - }; - - inline Quaternion::Quaternion() {} - inline Quaternion::Quaternion(zero_t) : q(zero) {} - inline Quaternion::Quaternion(float x, float y, float z, float w) : q(x, y, z, w) {} - inline Quaternion::Quaternion(Vector4::Arg v) : q(v) {} - - inline const Quaternion & Quaternion::operator=(Quaternion::Arg v) { q = v.q; return *this; } - - inline scalar Quaternion::x() const { return q.x(); } - inline scalar Quaternion::y() const { return q.y(); } - inline scalar Quaternion::z() const { return q.z(); } - inline scalar Quaternion::w() const { return q.w(); } - - inline const Vector4 & Quaternion::asVector() const { return q; } - inline Vector4 & Quaternion::asVector() { return q; } - - - inline Quaternion mul(Quaternion::Arg a, Quaternion::Arg b) - { - // @@ Efficient SIMD implementation? - return Quaternion( - + a.x() * b.w() + a.y()*b.z() - a.z()*b.y() + a.w()*b.x(), - - a.x() * b.z() + a.y()*b.w() + a.z()*b.x() + a.w()*b.y(), - + a.x() * b.y() - a.y()*b.x() + a.z()*b.w() + a.w()*b.z(), - - a.x() * b.x() - a.y()*b.y() - a.z()*b.z() + a.w()*b.w()); - } - - inline Quaternion scale(Quaternion::Arg q, float s) - { - return scale(q.asVector(), s); - } - inline Quaternion operator *(Quaternion::Arg q, float s) - { - return scale(q, s); - } - inline Quaternion operator *(float s, Quaternion::Arg q) - { - return scale(q, s); - } - - inline Quaternion scale(Quaternion::Arg q, Vector4::Arg s) - { - return scale(q.asVector(), s); - } - inline Quaternion operator *(Quaternion::Arg q, Vector4::Arg s) - { - return scale(q, s); - } - inline Quaternion operator *(Vector4::Arg s, Quaternion::Arg q) - { - return scale(q, s); - } - - inline Quaternion conjugate(Quaternion::Arg q) - { - return q * Vector4(-1, -1, -1, 1); - } - - inline float length(Quaternion::Arg q) - { - return length(q.asVector()); - } - - inline bool isNormalized(Quaternion::Arg q, float epsilon = NV_NORMAL_EPSILON) - { - return equal(length(q), 1, epsilon); - } - - inline Quaternion normalize(Quaternion::Arg q, float epsilon = NV_EPSILON) - { - float l = length(q); - nvDebugCheck(!isZero(l, epsilon)); - Quaternion n = scale(q, 1.0f / l); - nvDebugCheck(isNormalized(n)); - return n; - } - - inline Quaternion inverse(Quaternion::Arg q) - { - return conjugate(normalize(q)); - } - - /// Create a rotation quaternion for @a angle alpha around normal vector @a v. - inline Quaternion axisAngle(Vector3::Arg v, float alpha) - { - float s = sinf(alpha * 0.5f); - float c = cosf(alpha * 0.5f); - return Quaternion(Vector4(v * s, c)); - } - - -} // nv namespace - -#endif // NV_MATH_QUATERNION_H +// This code is in the public domain -- castanyo@yahoo.es + +#ifndef NV_MATH_QUATERNION_H +#define NV_MATH_QUATERNION_H + +#include +#include + +namespace nv +{ + + class NVMATH_CLASS Quaternion + { + public: + typedef Quaternion const & Arg; + + Quaternion(); + explicit Quaternion(zero_t); + Quaternion(float x, float y, float z, float w); + Quaternion(Vector4::Arg v); + + const Quaternion & operator=(Quaternion::Arg v); + + scalar x() const; + scalar y() const; + scalar z() const; + scalar w() const; + + const Vector4 & asVector() const; + Vector4 & asVector(); + + private: + Vector4 q; + }; + + inline Quaternion::Quaternion() {} + inline Quaternion::Quaternion(zero_t) : q(zero) {} + inline Quaternion::Quaternion(float x, float y, float z, float w) : q(x, y, z, w) {} + inline Quaternion::Quaternion(Vector4::Arg v) : q(v) {} + + inline const Quaternion & Quaternion::operator=(Quaternion::Arg v) { q = v.q; return *this; } + + inline scalar Quaternion::x() const { return q.x(); } + inline scalar Quaternion::y() const { return q.y(); } + inline scalar Quaternion::z() const { return q.z(); } + inline scalar Quaternion::w() const { return q.w(); } + + inline const Vector4 & Quaternion::asVector() const { return q; } + inline Vector4 & Quaternion::asVector() { return q; } + + + inline Quaternion mul(Quaternion::Arg a, Quaternion::Arg b) + { + // @@ Efficient SIMD implementation? + return Quaternion( + + a.x() * b.w() + a.y()*b.z() - a.z()*b.y() + a.w()*b.x(), + - a.x() * b.z() + a.y()*b.w() + a.z()*b.x() + a.w()*b.y(), + + a.x() * b.y() - a.y()*b.x() + a.z()*b.w() + a.w()*b.z(), + - a.x() * b.x() - a.y()*b.y() - a.z()*b.z() + a.w()*b.w()); + } + + inline Quaternion scale(Quaternion::Arg q, float s) + { + return scale(q.asVector(), s); + } + inline Quaternion operator *(Quaternion::Arg q, float s) + { + return scale(q, s); + } + inline Quaternion operator *(float s, Quaternion::Arg q) + { + return scale(q, s); + } + + inline Quaternion scale(Quaternion::Arg q, Vector4::Arg s) + { + return scale(q.asVector(), s); + } + /*inline Quaternion operator *(Quaternion::Arg q, Vector4::Arg s) + { + return scale(q, s); + } + inline Quaternion operator *(Vector4::Arg s, Quaternion::Arg q) + { + return scale(q, s); + }*/ + + inline Quaternion conjugate(Quaternion::Arg q) + { + return scale(q, Vector4(-1, -1, -1, 1)); + } + + inline float length(Quaternion::Arg q) + { + return length(q.asVector()); + } + + inline bool isNormalized(Quaternion::Arg q, float epsilon = NV_NORMAL_EPSILON) + { + return equal(length(q), 1, epsilon); + } + + inline Quaternion normalize(Quaternion::Arg q, float epsilon = NV_EPSILON) + { + float l = length(q); + nvDebugCheck(!isZero(l, epsilon)); + Quaternion n = scale(q, 1.0f / l); + nvDebugCheck(isNormalized(n)); + return n; + } + + inline Quaternion inverse(Quaternion::Arg q) + { + return conjugate(normalize(q)); + } + + /// Create a rotation quaternion for @a angle alpha around normal vector @a v. + inline Quaternion axisAngle(Vector3::Arg v, float alpha) + { + float s = sinf(alpha * 0.5f); + float c = cosf(alpha * 0.5f); + return Quaternion(Vector4(v * s, c)); + } + + +} // nv namespace + +#endif // NV_MATH_QUATERNION_H diff --git a/src/nvmath/TriBox.cpp b/src/nvmath/TriBox.cpp index 6c53e8f..61d69bb 100644 --- a/src/nvmath/TriBox.cpp +++ b/src/nvmath/TriBox.cpp @@ -13,7 +13,7 @@ /********************************************************/ #include -//#include +#include using namespace nv; @@ -96,7 +96,7 @@ static bool planeBoxOverlap(Vector3::Arg normal, Vector3::Arg vert, Vector3::Arg if(min>rad || max<-rad) return false; -bool triBoxOverlap(Vector3::Arg boxcenter, Vector3::Arg boxhalfsize, const Vector3 * triverts) +bool triBoxOverlap(Vector3::Arg boxcenter, Vector3::Arg boxhalfsize, const Triangle & tri) { // use separating axis theorem to test overlap between triangle and box // need to test for overlap in these directions: @@ -111,9 +111,9 @@ bool triBoxOverlap(Vector3::Arg boxcenter, Vector3::Arg boxhalfsize, const Vecto // This is the fastest branch on Sun. // move everything so that the boxcenter is in (0,0,0) - v0 = triverts[0] - boxcenter; - v1 = triverts[1] - boxcenter; - v2 = triverts[2] - boxcenter; + v0 = tri.v[0] - boxcenter; + v1 = tri.v[1] - boxcenter; + v2 = tri.v[2] - boxcenter; // Compute triangle edges. e0 = v1 - v0; // tri edge 0 @@ -170,7 +170,7 @@ bool triBoxOverlap(Vector3::Arg boxcenter, Vector3::Arg boxhalfsize, const Vecto } -bool triBoxOverlapNoBounds(Vector3::Arg boxcenter, Vector3::Arg boxhalfsize, const Vector3 * triverts) +bool triBoxOverlapNoBounds(Vector3::Arg boxcenter, Vector3::Arg boxhalfsize, const Triangle & tri) { // use separating axis theorem to test overlap between triangle and box // need to test for overlap in these directions: @@ -185,9 +185,9 @@ bool triBoxOverlapNoBounds(Vector3::Arg boxcenter, Vector3::Arg boxhalfsize, con // This is the fastest branch on Sun. // move everything so that the boxcenter is in (0,0,0) - v0 = triverts[0] - boxcenter; - v1 = triverts[1] - boxcenter; - v2 = triverts[2] - boxcenter; + v0 = tri.v[0] - boxcenter; + v1 = tri.v[1] - boxcenter; + v2 = tri.v[2] - boxcenter; // Compute triangle edges. e0 = v1 - v0; // tri edge 0 diff --git a/src/nvmath/Triangle.cpp b/src/nvmath/Triangle.cpp index 84819c7..f005297 100644 --- a/src/nvmath/Triangle.cpp +++ b/src/nvmath/Triangle.cpp @@ -6,26 +6,23 @@ using namespace nv; /// Tomas Möller, barycentric ray-triangle test. -bool Triangle::TestRay_Moller(Vector3::Arg orig, Vector3::Arg dir, float * out_t, float * out_u, float * out_v) +bool rayTest_Moller(const Triangle & t, Vector3::Arg orig, Vector3::Arg dir, float * out_t, float * out_u, float * out_v) { - Vector3 e1, e2, tvec, pvec, qvec; - float det, inv_det; - // find vectors for two edges sharing vert0 - e1 = v[1] - v[0]; - e2 = v[2] - v[0]; + Vector3 e1 = t.v[1] - t.v[0]; + Vector3 e2 = t.v[2] - t.v[0]; // begin calculating determinant - also used to calculate U parameter - pvec = cross(dir, e2); + Vector3 pvec = cross(dir, e2); // if determinant is near zero, ray lies in plane of triangle - det = dot(e1, pvec); + float det = dot(e1, pvec); if (det < -NV_EPSILON) { return false; } // calculate distance from vert0 to ray origin - tvec = orig - v[0]; + Vector3 tvec = orig - t.v[0]; // calculate U parameter and test bounds float u = dot(tvec, pvec); @@ -34,7 +31,7 @@ bool Triangle::TestRay_Moller(Vector3::Arg orig, Vector3::Arg dir, float * out_t } // prepare to test V parameter - qvec = cross(tvec, e1); + Vector3 qvec = cross(tvec, e1); // calculate V parameter and test bounds float v = dot(dir, qvec); @@ -43,7 +40,7 @@ bool Triangle::TestRay_Moller(Vector3::Arg orig, Vector3::Arg dir, float * out_t } // calculate t, scale parameters, ray intersects triangle - inv_det = 1.0f / det; + float inv_det = 1.0f / det; *out_t = dot(e2, qvec) * inv_det; *out_u = u * inv_det; // v *out_v = v * inv_det; // 1-(u+v) diff --git a/src/nvmath/Triangle.h b/src/nvmath/Triangle.h index e8d55e5..16772cd 100644 --- a/src/nvmath/Triangle.h +++ b/src/nvmath/Triangle.h @@ -3,146 +3,78 @@ #ifndef NV_MATH_TRIANGLE_H #define NV_MATH_TRIANGLE_H - #include #include #include -//#include namespace nv { -// Tomas Akenine-Möller box-triangle test. -NVMATH_API bool triBoxOverlap(Vector3::Arg boxcenter, Vector3::Arg boxhalfsize, const Vector3 * restrict triverts); -NVMATH_API bool triBoxOverlapNoBounds(Vector3::Arg boxcenter, Vector3::Arg boxhalfsize, const Vector3 * restrict triverts); - - -/// Triangle class with three vertices. -class Triangle -{ -public: - Triangle() {}; - - Triangle(const Vector3 & v0, const Vector3 & v1, const Vector3 & v2) + /// Triangle class with three vertices. + class Triangle { - v[0] = v0; - v[1] = v1; - v[2] = v2; - } + public: + Triangle() {}; - /// Get the bounds of the triangle. - Box bounds() const { - Box bounds; - bounds.clearBounds(); - bounds.addPointToBounds(v[0]); - bounds.addPointToBounds(v[1]); - bounds.addPointToBounds(v[2]); - return bounds; - } - -/* - /// Get barycentric coordinates of the given point in this triangle. - Vector3 barycentricCoordinates(Vector3::Arg p) - { - Vector3 bar; - - // p must lie in the triangle plane. - Plane plane; - plane.set(v[0], v[1], v[2]); - nvCheck( equalf(plane.Distance(p), 0.0f) ); - - Vector3 n; - - // Compute signed area of triangle - n = cross(v[1] - v[0], p - v[0]); - bar.x = length(n); - if (dot(n, plane.vector) < 0) { - bar->x = -bar->x; + Triangle(Vector3::Arg v0, Vector3::Arg v1, Vector3::Arg v2) + { + v[0] = v0; + v[1] = v1; + v[2] = v2; } - // Compute signed area of triangle - n = cross(v[2] - v[1], p - v[1]); - bar->y = length(cross(e, d)); - if (dot(n, plane.vector) < 0) { - bar->y = -bar->y; + /// Get the bounds of the triangle. + Box bounds() const + { + Box bounds; + bounds.clearBounds(); + bounds.addPointToBounds(v[0]); + bounds.addPointToBounds(v[1]); + bounds.addPointToBounds(v[2]); + return bounds; } - // Compute signed area of triangle - n = cross(v[0] - v[2], p - v[2]); - bar->z = length(n); - if (dot(n, plane.vector) < 0) { - bar->z = -bar->z; + Vector4 plane() const + { + Vector3 n = cross(v[1]-v[0], v[2]-v[0]); + return Vector4(n, dot(n, v[0])); } - // We cannot just do this because we need the signed areas. - // bar->x = Vector3Area(e0, d0); - // bar->y = Vector3Area(e1, d1); - // bar->z = Vector3Area(e2, d2); + Vector3 v[3]; + }; - // bar->x = Vector3TripleProduct(v[1], v[2], p); - // bar->y = Vector3TripleProduct(v[2], v[0], p); - // bar->z = Vector3TripleProduct(v[0], v[1], p); - } -*/ + // Tomas Akenine-Möller box-triangle test. + NVMATH_API bool triBoxOverlap(Vector3::Arg boxcenter, Vector3::Arg boxhalfsize, const Triangle & triangle); + NVMATH_API bool triBoxOverlapNoBounds(Vector3::Arg boxcenter, Vector3::Arg boxhalfsize, const Triangle & triangle); + // Moller ray triangle test. - bool TestRay_Moller(const Vector3 & orig, const Vector3 & dir, float * out_t, float * out_u, float * out_v); + NVMATH_API bool rayTest_Moller(const Triangle & t, Vector3::Arg orig, Vector3::Arg dir, float * out_t, float * out_u, float * out_v); - Vector3 v[3]; -}; - - -#if 0 - -/** A planar triangle. */ -class Triangle2 { -public: - - Triangle2() {}; - Triangle2(const Vec2 & v0, const Vec2 & v1, const Vec2 & v2) { - v[0] = v0; - v[1] = v1; - v[2] = v2; + inline bool rayTest(const Triangle & t, Vector3::Arg orig, Vector3::Arg dir, float * out_t, float * out_u, float * out_v) + { + rayTest_Moller(t, orig, dir, out_t, out_u, out_v); + } + + inline bool overlap(const Triangle & t, const Box & b) + { + Vector3 center = b.center(); + Vector3 extents = b.extents(); + return triBoxOverlap(center, extents, t); } - /** Get the barycentric coordinates of the given point for this triangle. - * http://stevehollasch.com/cgindex/math/barycentric.html - */ - void GetBarycentricCoordinates(const Vec2 & p, Vector3 * bar) const { - float denom = 1.0f / (v[1].x - v[0].x) * (v[2].y - v[0].y) - (v[2].x - v[0].x) * (v[1].y - v[0].y); - bar->x = ((v[1].x - p.x) * (v[2].y - p.y) - (v[2].x - p.x) * (v[1].y - p.y)) * denom; - bar->y = ((v[2].x - p.x) * (v[0].y - p.y) - (v[0].x - p.x) * (v[2].y - p.y)) * denom; - //bar->z = ((v[0].x - p.x) * (v[1].y - p.y) - (v[1].x - p.x) * (v[0].y - p.y)) * denom; - bar->z = 1 - bar->x - bar->y; + inline bool overlap(const Box & b, const Triangle & t) + { + return overlap(t, b); } - - Vec2 v[3]; -}; - -#endif // 0 - - -inline bool overlap(const Triangle & t, const Box & b) -{ - Vector3 center = b.center(); - Vector3 extents = b.extents(); - return triBoxOverlap(center, extents, t.v); -} - -inline bool Overlap(const Box & b, const Triangle & t) -{ - return overlap(t, b); -} - - -inline bool overlapNoBounds(const Triangle & t, const Box & b) -{ - Vector3 center = b.center(); - Vector3 extents = b.extents(); - return triBoxOverlapNoBounds(center, extents, t.v); -} + inline bool overlapNoBounds(const Triangle & t, const Box & b) + { + Vector3 center = b.center(); + Vector3 extents = b.extents(); + return triBoxOverlapNoBounds(center, extents, t); + } } // nv namespace