Merge changes from private tree.
Eliminate files that are not needed for NVTT.
This commit is contained in:
castano
@ -1,228 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#include <nvmath/Basis.h>
|
||||
|
||||
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 = nv::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
|
||||
{
|
||||
#if 0
|
||||
tangent = nv::normalize(tangent, epsilon);
|
||||
bitangent -= tangent * dot(tangent, bitangent);
|
||||
|
||||
if (length(bitangent) < epsilon)
|
||||
{
|
||||
bitangent = cross(tangent, normal);
|
||||
nvCheck(isNormalized(bitangent));
|
||||
}
|
||||
else
|
||||
{
|
||||
bitangent = nv::normalize(bitangent, epsilon);
|
||||
}
|
||||
#else
|
||||
if (length(bitangent) < epsilon)
|
||||
{
|
||||
bitangent = cross(tangent, normal);
|
||||
nvCheck(isNormalized(bitangent));
|
||||
}
|
||||
else
|
||||
{
|
||||
tangent = nv::normalize(tangent);
|
||||
bitangent = nv::normalize(bitangent);
|
||||
|
||||
Vector3 bisector;
|
||||
if (length(tangent + bitangent) < epsilon)
|
||||
{
|
||||
bisector = tangent;
|
||||
}
|
||||
else
|
||||
{
|
||||
bisector = nv::normalize(tangent + bitangent);
|
||||
}
|
||||
Vector3 axis = cross(bisector, normal);
|
||||
|
||||
nvDebugCheck(isNormalized(axis, epsilon));
|
||||
nvDebugCheck(equal(dot(axis, tangent), -dot(axis, bitangent), epsilon));
|
||||
|
||||
if (dot(axis, tangent) > 0)
|
||||
{
|
||||
tangent = nv::normalize(bisector + axis);
|
||||
bitangent = nv::normalize(bisector - axis);
|
||||
}
|
||||
else
|
||||
{
|
||||
tangent = nv::normalize(bisector - axis);
|
||||
bitangent = nv::normalize(bisector + axis);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
/*// Check vector lengths.
|
||||
if (!isNormalized(normal, epsilon))
|
||||
{
|
||||
nvDebug("%f %f %f\n", normal.x(), normal.y(), normal.z());
|
||||
nvDebug("%f %f %f\n", tangent.x(), tangent.y(), tangent.z());
|
||||
nvDebug("%f %f %f\n", bitangent.x(), bitangent.y(), bitangent.z());
|
||||
}*/
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
bool Basis::isValid() const
|
||||
{
|
||||
if (equal(normal, Vector3(zero))) return false;
|
||||
if (equal(tangent, Vector3(zero))) return false;
|
||||
if (equal(bitangent, Vector3(zero))) return false;
|
||||
|
||||
if (equal(determinant(), 0.0f)) return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
/// 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 Cramer'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();
|
||||
nvDebugCheck(!equal(det, 0.0f, 0.0f));
|
||||
|
||||
const float idet = 1.0f / det;
|
||||
|
||||
// Rows of the inverse matrix.
|
||||
Vector3 r0(
|
||||
(bitangent.y() * normal.z() - bitangent.z() * normal.y()),
|
||||
-(bitangent.x() * normal.z() - bitangent.z() * normal.x()),
|
||||
(bitangent.x() * normal.y() - bitangent.y() * normal.x()));
|
||||
|
||||
Vector3 r1(
|
||||
-(tangent.y() * normal.z() - tangent.z() * normal.y()),
|
||||
(tangent.x() * normal.z() - tangent.z() * normal.x()),
|
||||
-(tangent.x() * normal.y() - tangent.y() * normal.x()));
|
||||
|
||||
Vector3 r2(
|
||||
(tangent.y() * bitangent.z() - tangent.z() * bitangent.y()),
|
||||
-(tangent.x() * bitangent.z() - tangent.z() * bitangent.x()),
|
||||
(tangent.x() * bitangent.y() - tangent.y() * bitangent.x()));
|
||||
|
||||
return Vector3(dot(v, r0), dot(v, r1), dot(v, r2)) * idet;
|
||||
}
|
||||
|
||||
|
||||
@ -1,81 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_BASIS_H
|
||||
#define NV_MATH_BASIS_H
|
||||
|
||||
#include <nvmath/nvmath.h>
|
||||
#include <nvmath/Vector.h>
|
||||
#include <nvmath/Matrix.h>
|
||||
|
||||
namespace nv
|
||||
{
|
||||
|
||||
/// Basis class to compute tangent space basis, ortogonalizations and to
|
||||
/// transform vectors from one space to another.
|
||||
class Basis
|
||||
{
|
||||
public:
|
||||
|
||||
/// 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();
|
||||
}
|
||||
|
||||
bool isValid() const;
|
||||
|
||||
// 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
|
||||
@ -1,97 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#include "Bezier.h"
|
||||
|
||||
|
||||
using namespace nv;
|
||||
|
||||
|
||||
static void deCasteljau(float u, Vector3::Arg p0, Vector3::Arg p1, Vector3::Arg p2, Vector3::Arg p3, Vector3 * p)
|
||||
{
|
||||
Vector3 q0 = lerp(p0, p1, u);
|
||||
Vector3 q1 = lerp(p1, p2, u);
|
||||
Vector3 q2 = lerp(p2, p3, u);
|
||||
Vector3 r0 = lerp(q0, q1, u);
|
||||
Vector3 r1 = lerp(q1, q2, u);
|
||||
|
||||
*p = lerp(r0, r1, u);
|
||||
}
|
||||
|
||||
static void deCasteljau(float u, Vector3::Arg p0, Vector3::Arg p1, Vector3::Arg p2, Vector3::Arg p3, Vector3 * p, Vector3 * dp)
|
||||
{
|
||||
Vector3 q0 = lerp(p0, p1, u);
|
||||
Vector3 q1 = lerp(p1, p2, u);
|
||||
Vector3 q2 = lerp(p2, p3, u);
|
||||
Vector3 r0 = lerp(q0, q1, u);
|
||||
Vector3 r1 = lerp(q1, q2, u);
|
||||
|
||||
*dp = r0 - r1;
|
||||
*p = lerp(r0, r1, u);
|
||||
}
|
||||
|
||||
|
||||
void nv::evaluateCubicBezierPatch(float u, float v,
|
||||
Vector3::Arg p0, Vector3::Arg p1, Vector3::Arg p2, Vector3::Arg p3,
|
||||
Vector3::Arg p4, Vector3::Arg p5, Vector3::Arg p6, Vector3::Arg p7,
|
||||
Vector3::Arg p8, Vector3::Arg p9, Vector3::Arg pA, Vector3::Arg pB,
|
||||
Vector3::Arg pC, Vector3::Arg pD, Vector3::Arg pE, Vector3::Arg pF,
|
||||
Vector3 * pos, Vector3 * du, Vector3 * dv)
|
||||
{
|
||||
#if 0
|
||||
Vector2 L0(1-u,1-v);
|
||||
Vector2 L1(u,v);
|
||||
|
||||
Vector2 Q0 = L0 * L0;
|
||||
Vector2 Q1 = 2 * L0 * L1;
|
||||
Vector2 Q2 = L1 * L1;
|
||||
|
||||
Vector2 B0 = L0 * L0 * L0;
|
||||
Vector2 B1 = 3 * L0 * L0 * L1;
|
||||
Vector2 B2 = 3 * L1 * L1 * L0;
|
||||
Vector2 B3 = L1 * L1 * L1;
|
||||
|
||||
*pos =
|
||||
(B0.x() * p0 + B1.x() * p1 + B2.x() * p2 + B3.x() * p3) * B0.y() +
|
||||
(B0.x() * p4 + B1.x() * p5 + B2.x() * p6 + B3.x() * p7) * B1.y() +
|
||||
(B0.x() * p8 + B1.x() * p9 + B2.x() * pA + B3.x() * pB) * B2.y() +
|
||||
(B0.x() * pC + B1.x() * pD + B2.x() * pE + B3.x() * pF) * B3.y();
|
||||
|
||||
*du =
|
||||
((p0-p1) * B0.y() + (p4-p5) * B1.y() + (p8-p9) * B2.y() + (pC-pD) * B3.y()) * Q0.x() +
|
||||
((p1-p2) * B0.y() + (p5-p6) * B1.y() + (p9-pA) * B2.y() + (pD-pE) * B3.y()) * Q1.x() +
|
||||
((p2-p3) * B0.y() + (p6-p7) * B1.y() + (pA-pB) * B2.y() + (pE-pF) * B3.y()) * Q2.x();
|
||||
|
||||
*dv =
|
||||
((p0-p4) * B0.x() + (p1-p5) * B1.x() + (p2-p6) * B2.x() + (p3-p7) * B3.x()) * Q0.y() +
|
||||
((p4-p8) * B0.x() + (p5-p9) * B1.x() + (p6-pA) * B2.x() + (p7-pB) * B3.x()) * Q1.y() +
|
||||
((p8-pC) * B0.x() + (p9-pD) * B1.x() + (pA-pE) * B2.x() + (pB-pF) * B3.x()) * Q2.y();
|
||||
#else
|
||||
Vector3 t0, t1, t2, t3;
|
||||
Vector3 q0, q1, q2, q3;
|
||||
|
||||
deCasteljau(u, p0, p1, p2, p3, &q0, &t0);
|
||||
deCasteljau(u, p4, p5, p6, p7, &q1, &t1);
|
||||
deCasteljau(u, p8, p9, pA, pB, &q2, &t2);
|
||||
deCasteljau(u, pC, pD, pE, pF, &q3, &t3);
|
||||
|
||||
deCasteljau(v, q0, q1, q2, q3, pos, dv);
|
||||
deCasteljau(v, t0, t1, t2, t3, du);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
void nv::degreeElevate(Vector3::Arg p0, Vector3::Arg p1, Vector3::Arg p2, Vector3::Arg p3,
|
||||
Vector3 * q0, Vector3 * q1, Vector3 * q2, Vector3 * q3, Vector3 * q4)
|
||||
{
|
||||
}
|
||||
|
||||
void nv::evaluateQuarticBezierTriangle(float u, float v,
|
||||
Vector3::Arg p0, Vector3::Arg p1, Vector3::Arg p2, Vector3::Arg p3, Vector3::Arg p4,
|
||||
Vector3::Arg p5, Vector3::Arg p6, Vector3::Arg p7, Vector3::Arg p8,
|
||||
Vector3::Arg p9, Vector3::Arg pA, Vector3::Arg pB,
|
||||
Vector3::Arg pC, Vector3::Arg pD,
|
||||
Vector3::Arg pE,
|
||||
Vector3 * pos, Vector3 * du, Vector3 * dv)
|
||||
{
|
||||
}
|
||||
|
||||
@ -1,33 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_BEZIER_H
|
||||
#define NV_MATH_BEZIER_H
|
||||
|
||||
#include <nvmath/nvmath.h>
|
||||
#include <nvmath/Vector.h>
|
||||
|
||||
namespace nv
|
||||
{
|
||||
|
||||
void evaluateCubicBezierPatch(float u, float v,
|
||||
Vector3::Arg p0, Vector3::Arg p1, Vector3::Arg p2, Vector3::Arg p3,
|
||||
Vector3::Arg p4, Vector3::Arg p5, Vector3::Arg p6, Vector3::Arg p7,
|
||||
Vector3::Arg p8, Vector3::Arg p9, Vector3::Arg pA, Vector3::Arg pB,
|
||||
Vector3::Arg pC, Vector3::Arg pD, Vector3::Arg pE, Vector3::Arg pF,
|
||||
Vector3 * pos, Vector3 * du, Vector3 * dv);
|
||||
|
||||
void degreeElevate(Vector3::Arg p0, Vector3::Arg p1, Vector3::Arg p2, Vector3::Arg p3,
|
||||
Vector3 * q0, Vector3 * q1, Vector3 * q2, Vector3 * q3, Vector3 * q4);
|
||||
|
||||
void evaluateQuarticBezierTriangle(float u, float v,
|
||||
Vector3::Arg p0, Vector3::Arg p1, Vector3::Arg p2, Vector3::Arg p3, Vector3::Arg p4,
|
||||
Vector3::Arg p5, Vector3::Arg p6, Vector3::Arg p7, Vector3::Arg p8,
|
||||
Vector3::Arg p9, Vector3::Arg pA, Vector3::Arg pB,
|
||||
Vector3::Arg pC, Vector3::Arg pD,
|
||||
Vector3::Arg pE,
|
||||
Vector3 * pos, Vector3 * du, Vector3 * dv);
|
||||
|
||||
|
||||
} // nv namespace
|
||||
|
||||
#endif // NV_MATH_BEZIER_H
|
||||
@ -4,24 +4,12 @@ SET(MATH_SRCS
|
||||
nvmath.h
|
||||
Vector.h
|
||||
Matrix.h
|
||||
Quaternion.h
|
||||
Plane.h Plane.cpp
|
||||
Box.h
|
||||
Color.h
|
||||
Frustum.h Frustum.cpp
|
||||
Montecarlo.h Montecarlo.cpp
|
||||
Random.h Random.cpp
|
||||
SphericalHarmonic.h SphericalHarmonic.cpp
|
||||
Basis.h Basis.cpp
|
||||
Triangle.h Triangle.cpp TriBox.cpp
|
||||
Polygon.h Polygon.cpp
|
||||
TypeSerialization.h TypeSerialization.cpp
|
||||
Sparse.h Sparse.cpp
|
||||
Solver.h Solver.cpp
|
||||
KahanSum.h
|
||||
Half.h Half.cpp
|
||||
Fitting.h Fitting.cpp
|
||||
Bezier.h Bezier.cpp)
|
||||
Fitting.h Fitting.cpp)
|
||||
|
||||
INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
|
||||
@ -4,57 +4,14 @@
|
||||
|
||||
#include <nvcore/Algorithms.h> // max
|
||||
#include <nvcore/Containers.h> // swap
|
||||
|
||||
#include <float.h> // FLT_MAX
|
||||
|
||||
using namespace nv;
|
||||
|
||||
|
||||
Vector3 nv::ComputeCentroid(int n, const Vector3 * points, const float * weights, Vector3::Arg metric)
|
||||
// @@ Move to EigenSolver.h
|
||||
static inline Vector3 firstEigenVector_PowerMethod(const float *__restrict matrix)
|
||||
{
|
||||
Vector3 centroid(zero);
|
||||
float total = 0.0f;
|
||||
|
||||
for (int i = 0; i < n; i++)
|
||||
{
|
||||
total += weights[i];
|
||||
centroid += weights[i]*points[i];
|
||||
}
|
||||
centroid /= total;
|
||||
|
||||
return centroid;
|
||||
}
|
||||
|
||||
|
||||
void nv::ComputeCovariance(int n, const Vector3 * points, const float * weights, Vector3::Arg metric, float * covariance)
|
||||
{
|
||||
// compute the centroid
|
||||
Vector3 centroid = ComputeCentroid(n, points, weights, metric);
|
||||
|
||||
// compute covariance matrix
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
covariance[i] = 0.0f;
|
||||
}
|
||||
|
||||
for (int i = 0; i < n; i++)
|
||||
{
|
||||
Vector3 a = (points[i] - centroid) * metric;
|
||||
Vector3 b = weights[i]*a;
|
||||
|
||||
covariance[0] += a.x()*b.x();
|
||||
covariance[1] += a.x()*b.y();
|
||||
covariance[2] += a.x()*b.z();
|
||||
covariance[3] += a.y()*b.y();
|
||||
covariance[4] += a.y()*b.z();
|
||||
covariance[5] += a.z()*b.z();
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 nv::ComputePrincipalComponent(int n, const Vector3 * points, const float * weights, Vector3::Arg metric)
|
||||
{
|
||||
float matrix[6];
|
||||
ComputeCovariance(n, points, weights, metric, matrix);
|
||||
|
||||
if (matrix[0] == 0 || matrix[3] == 0 || matrix[5] == 0)
|
||||
{
|
||||
return Vector3(zero);
|
||||
@ -78,13 +35,135 @@ Vector3 nv::ComputePrincipalComponent(int n, const Vector3 * points, const float
|
||||
}
|
||||
|
||||
|
||||
|
||||
int nv::Compute4Means(int n, const Vector3 * points, const float * weights, Vector3::Arg metric, Vector3 * cluster)
|
||||
Vector3 nv::Fit::computeCentroid(int n, const Vector3 *__restrict points)
|
||||
{
|
||||
Vector3 centroid(zero);
|
||||
|
||||
for (int i = 0; i < n; i++)
|
||||
{
|
||||
centroid += points[i];
|
||||
}
|
||||
centroid /= float(n);
|
||||
|
||||
return centroid;
|
||||
}
|
||||
|
||||
Vector3 nv::Fit::computeCentroid(int n, const Vector3 *__restrict points, const float *__restrict weights, Vector3::Arg metric)
|
||||
{
|
||||
Vector3 centroid(zero);
|
||||
float total = 0.0f;
|
||||
|
||||
for (int i = 0; i < n; i++)
|
||||
{
|
||||
total += weights[i];
|
||||
centroid += weights[i]*points[i];
|
||||
}
|
||||
centroid /= total;
|
||||
|
||||
return centroid;
|
||||
}
|
||||
|
||||
|
||||
Vector3 nv::Fit::computeCovariance(int n, const Vector3 *__restrict points, float *__restrict covariance)
|
||||
{
|
||||
// compute the centroid
|
||||
Vector3 centroid = computeCentroid(n, points);
|
||||
|
||||
// compute covariance matrix
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
covariance[i] = 0.0f;
|
||||
}
|
||||
|
||||
for (int i = 0; i < n; i++)
|
||||
{
|
||||
Vector3 v = points[i] - centroid;
|
||||
|
||||
covariance[0] += v.x() * v.x();
|
||||
covariance[1] += v.x() * v.y();
|
||||
covariance[2] += v.x() * v.z();
|
||||
covariance[3] += v.y() * v.y();
|
||||
covariance[4] += v.y() * v.z();
|
||||
covariance[5] += v.z() * v.z();
|
||||
}
|
||||
|
||||
return centroid;
|
||||
}
|
||||
|
||||
Vector3 nv::Fit::computeCovariance(int n, const Vector3 *__restrict points, const float *__restrict weights, Vector3::Arg metric, float *__restrict covariance)
|
||||
{
|
||||
// compute the centroid
|
||||
Vector3 centroid = computeCentroid(n, points, weights, metric);
|
||||
|
||||
// compute covariance matrix
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
covariance[i] = 0.0f;
|
||||
}
|
||||
|
||||
for (int i = 0; i < n; i++)
|
||||
{
|
||||
Vector3 a = (points[i] - centroid) * metric;
|
||||
Vector3 b = weights[i]*a;
|
||||
|
||||
covariance[0] += a.x()*b.x();
|
||||
covariance[1] += a.x()*b.y();
|
||||
covariance[2] += a.x()*b.z();
|
||||
covariance[3] += a.y()*b.y();
|
||||
covariance[4] += a.y()*b.z();
|
||||
covariance[5] += a.z()*b.z();
|
||||
}
|
||||
|
||||
return centroid;
|
||||
}
|
||||
|
||||
Vector3 nv::Fit::computePrincipalComponent(int n, const Vector3 *__restrict points)
|
||||
{
|
||||
float matrix[6];
|
||||
computeCovariance(n, points, matrix);
|
||||
|
||||
return firstEigenVector_PowerMethod(matrix);
|
||||
}
|
||||
|
||||
Vector3 nv::Fit::computePrincipalComponent(int n, const Vector3 *__restrict points, const float *__restrict weights, Vector3::Arg metric)
|
||||
{
|
||||
float matrix[6];
|
||||
computeCovariance(n, points, weights, metric, matrix);
|
||||
|
||||
return firstEigenVector_PowerMethod(matrix);
|
||||
}
|
||||
|
||||
|
||||
Plane nv::Fit::bestPlane(int n, const Vector3 *__restrict points)
|
||||
{
|
||||
// compute the centroid and covariance
|
||||
float matrix[6];
|
||||
Vector3 centroid = computeCovariance(n, points, matrix);
|
||||
|
||||
if (matrix[0] == 0 || matrix[3] == 0 || matrix[5] == 0)
|
||||
{
|
||||
// If no plane defined, then return a horizontal plane.
|
||||
return Plane(Vector3(0, 0, 1), centroid);
|
||||
}
|
||||
|
||||
#pragma message(NV_FILE_LINE "TODO: need to write an eigensolver!")
|
||||
|
||||
// - Numerical Recipes in C is a good reference. Householder transforms followed by QL decomposition seems to be the best approach.
|
||||
// - The one from magic-tools is now LGPL. For the 3D case it uses a cubic root solver, which is not very accurate.
|
||||
// - Charles' Galaxy3 contains an implementation of the tridiagonalization method, but is under BPL.
|
||||
|
||||
//EigenSolver3 solver(matrix);
|
||||
|
||||
return Plane();
|
||||
}
|
||||
|
||||
|
||||
int nv::Fit::compute4Means(int n, const Vector3 *__restrict points, const float *__restrict weights, Vector3::Arg metric, Vector3 *__restrict cluster)
|
||||
{
|
||||
Vector3 centroid = ComputeCentroid(n, points, weights, metric);
|
||||
|
||||
// Compute principal component.
|
||||
Vector3 principal = ComputePrincipalComponent(n, points, weights, metric);
|
||||
float matrix[6];
|
||||
Vector3 centroid = computeCovariance(n, points, weights, metric, matrix);
|
||||
Vector3 principal = firstEigenVector_PowerMethod(matrix);
|
||||
|
||||
// Pick initial solution.
|
||||
int mini, maxi;
|
||||
|
||||
@ -5,16 +5,26 @@
|
||||
|
||||
#include <nvmath/nvmath.h>
|
||||
#include <nvmath/Vector.h>
|
||||
#include <nvmath/Plane.h>
|
||||
|
||||
namespace nv
|
||||
{
|
||||
namespace Fit
|
||||
{
|
||||
Vector3 computeCentroid(int n, const Vector3 * points);
|
||||
Vector3 computeCentroid(int n, const Vector3 * points, const float * weights, Vector3::Arg metric);
|
||||
|
||||
Vector3 ComputeCentroid(int n, const Vector3 * points, const float * weights, Vector3::Arg metric);
|
||||
void ComputeCovariance(int n, const Vector3 * points, const float * weights, Vector3::Arg metric, float * covariance);
|
||||
Vector3 ComputePrincipalComponent(int n, const Vector3 * points, const float * weights, Vector3::Arg metric);
|
||||
Vector3 computeCovariance(int n, const Vector3 * points, float * covariance);
|
||||
Vector3 computeCovariance(int n, const Vector3 * points, const float * weights, Vector3::Arg metric, float * covariance);
|
||||
|
||||
// Returns number of clusters [1-4].
|
||||
int Compute4Means(int n, const Vector3 * points, const float * weights, Vector3::Arg metric, Vector3 * cluster);
|
||||
Vector3 computePrincipalComponent(int n, const Vector3 * points);
|
||||
Vector3 computePrincipalComponent(int n, const Vector3 * points, const float * weights, Vector3::Arg metric);
|
||||
|
||||
Plane bestPlane(int n, const Vector3 * points);
|
||||
|
||||
// Returns number of clusters [1-4].
|
||||
int compute4Means(int n, const Vector3 * points, const float * weights, Vector3::Arg metric, Vector3 * cluster);
|
||||
}
|
||||
|
||||
} // nv namespace
|
||||
|
||||
|
||||
@ -1,76 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#include "Frustum.h"
|
||||
#include "Box.h"
|
||||
#include "Matrix.h"
|
||||
|
||||
namespace nv
|
||||
{
|
||||
Frustum::Frustum(float fovy, float aspect, float near, float far)
|
||||
{
|
||||
// Define a frustum looking along the -ve z axis.
|
||||
planes[NEAR] = Plane(Vector3(0,0,1), near);
|
||||
planes[FAR] = Plane(Vector3(0,0,-1), far);
|
||||
|
||||
const Vector3 origin(zero);
|
||||
const float tanfy = tan(fovy/2.0f);
|
||||
planes[TOP] = Plane(Vector3( 1.0f, 0.0f, tanfy * aspect), origin);
|
||||
planes[BOTTOM] = Plane(Vector3(-1.0f, 0.0f, tanfy * aspect), origin);
|
||||
|
||||
planes[RIGHT] = Plane(Vector3(0.0f, 1.0f, tanfy), origin);
|
||||
planes[LEFT] = Plane(Vector3(0.0f, -1.0f, tanfy), origin);
|
||||
|
||||
for(int p = 0; p < Frustum::PLANE_COUNT; ++p)
|
||||
planes[p] = normalize(planes[p]);
|
||||
}
|
||||
|
||||
static void getAllCorners(const Box& boxy, Vector3 c[])
|
||||
{
|
||||
c[0] = boxy.minCorner();
|
||||
c[1] = boxy.maxCorner();
|
||||
c[2] = Vector3(c[0].x(), c[1].y(), c[0].z());
|
||||
c[3] = Vector3(c[0].x(), c[1].y(), c[1].z());
|
||||
c[4] = Vector3(c[0].x(), c[0].y(), c[1].z());
|
||||
c[5] = Vector3(c[1].x(), c[1].y(), c[0].z());
|
||||
c[6] = Vector3(c[1].x(), c[0].y(), c[0].z());
|
||||
c[7] = Vector3(c[1].x(), c[0].y(), c[1].z());
|
||||
}
|
||||
|
||||
bool Frustum::intersects(const Box& boxy) const
|
||||
{
|
||||
const int N_CORNERS = 8;
|
||||
Vector3 boxCorners[N_CORNERS];
|
||||
getAllCorners(boxy, boxCorners);
|
||||
|
||||
// test all 8 corners against the 6 sides
|
||||
// if all points are behind 1 specific plane, we are out
|
||||
// if we are in with all points, then we are fully in
|
||||
for(int p = 0; p < Frustum::PLANE_COUNT; ++p)
|
||||
{
|
||||
int iInCount = N_CORNERS;
|
||||
|
||||
for(int i = 0; i < N_CORNERS; ++i)
|
||||
{
|
||||
// test point [i] against the planes
|
||||
if (distance(planes[p], boxCorners[i]) > 0.0f)
|
||||
--iInCount;
|
||||
}
|
||||
|
||||
// were all the points outside of plane p?
|
||||
if (iInCount == 0)
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
Frustum transformFrustum(const Matrix& m, const Frustum& f)
|
||||
{
|
||||
Frustum result;
|
||||
|
||||
for (int i=0; i!=Frustum::PLANE_COUNT; ++i)
|
||||
result.planes[i] = transformPlane(m, f.planes[i]);
|
||||
|
||||
return result;
|
||||
}
|
||||
}
|
||||
@ -1,34 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_FRUSTUM_H
|
||||
#define NV_MATH_FRUSTUM_H
|
||||
|
||||
#include <nvmath/Plane.h>
|
||||
|
||||
namespace nv
|
||||
{
|
||||
class Box;
|
||||
|
||||
class NVMATH_CLASS Frustum
|
||||
{
|
||||
public:
|
||||
// Construct a frustum from typical view parameters. fovy has the same meaning as for glut_perspective_reshaper.
|
||||
// The resulting frustum is centred at the origin, pointing along the z-axis.
|
||||
Frustum() {}
|
||||
Frustum(float fovy, float aspect, float near, float far);
|
||||
|
||||
// Unlike some intersection methods, we don't bother to distinguish intersects
|
||||
// from contains. A true result could indicate either.
|
||||
bool intersects(const Box&) const;
|
||||
|
||||
private:
|
||||
friend Frustum transformFrustum(const Matrix&, const Frustum&);
|
||||
|
||||
enum PlaneIndices { NEAR, LEFT, RIGHT, TOP, BOTTOM, FAR, PLANE_COUNT };
|
||||
Plane planes[PLANE_COUNT];
|
||||
};
|
||||
|
||||
Frustum transformFrustum(const Matrix&, const Frustum&);
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -1,38 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_KAHANSUM_H
|
||||
#define NV_MATH_KAHANSUM_H
|
||||
|
||||
#include <nvmath/nvmath.h>
|
||||
|
||||
namespace nv
|
||||
{
|
||||
|
||||
class KahanSum
|
||||
{
|
||||
public:
|
||||
KahanSum() : accum(0.0f), err(0) {};
|
||||
|
||||
void add(float f)
|
||||
{
|
||||
float compensated = f + err;
|
||||
float tmp = accum + compensated;
|
||||
err = accum - tmp;
|
||||
err += compensated;
|
||||
accum = tmp;
|
||||
}
|
||||
|
||||
float sum() const
|
||||
{
|
||||
return accum;
|
||||
}
|
||||
|
||||
private:
|
||||
float accum;
|
||||
float err;
|
||||
};
|
||||
|
||||
} // nv namespace
|
||||
|
||||
|
||||
#endif // NV_MATH_KAHANSUM_H
|
||||
@ -1,168 +0,0 @@
|
||||
// This code is in the public domain -- Ignacio Casta<74>o <castanyo@yahoo.es>
|
||||
|
||||
#include <nvmath/Polygon.h>
|
||||
|
||||
#include <nvmath/Triangle.h>
|
||||
#include <nvmath/Plane.h>
|
||||
|
||||
using namespace nv;
|
||||
|
||||
|
||||
Polygon::Polygon()
|
||||
{
|
||||
}
|
||||
|
||||
Polygon::Polygon(const Triangle & t)
|
||||
{
|
||||
pointArray.resize(3);
|
||||
pointArray[0] = t.v[0];
|
||||
pointArray[1] = t.v[1];
|
||||
pointArray[2] = t.v[2];
|
||||
}
|
||||
|
||||
Polygon::Polygon(const Vector3 * points, uint vertexCount)
|
||||
{
|
||||
pointArray.resize(vertexCount);
|
||||
|
||||
for (uint i = 0; i < vertexCount; i++)
|
||||
{
|
||||
pointArray[i] = points[i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/// Compute polygon area.
|
||||
float Polygon::area() const
|
||||
{
|
||||
float total = 0;
|
||||
|
||||
const uint pointCount = pointArray.count();
|
||||
for (uint i = 2; i < pointCount; i++)
|
||||
{
|
||||
Vector3 v1 = pointArray[i-1] - pointArray[0];
|
||||
Vector3 v2 = pointArray[i] - pointArray[0];
|
||||
|
||||
total += 0.5f * length(cross(v1, v2));
|
||||
}
|
||||
|
||||
return total;
|
||||
}
|
||||
|
||||
/// Get the bounds of the polygon.
|
||||
Box Polygon::bounds() const
|
||||
{
|
||||
Box bounds;
|
||||
bounds.clearBounds();
|
||||
foreach(p, pointArray)
|
||||
{
|
||||
bounds.addPointToBounds(pointArray[p]);
|
||||
}
|
||||
return bounds;
|
||||
}
|
||||
|
||||
|
||||
/// Get the plane of the polygon.
|
||||
Plane Polygon::plane() const
|
||||
{
|
||||
// @@ Do something better than this?
|
||||
Vector3 n = cross(pointArray[1] - pointArray[0], pointArray[2] - pointArray[0]);
|
||||
return Vector4(n, dot(n, pointArray[0]));
|
||||
}
|
||||
|
||||
|
||||
/// Clip polygon to box.
|
||||
uint Polygon::clipTo(const Box & box)
|
||||
{
|
||||
const Plane posX( 1, 0, 0, box.maxCorner().x());
|
||||
const Plane negX(-1, 0, 0,-box.minCorner().x());
|
||||
const Plane posY( 0, 1, 0, box.maxCorner().y());
|
||||
const Plane negY( 0,-1, 0,-box.minCorner().y());
|
||||
const Plane posZ( 0, 0, 1, box.maxCorner().z());
|
||||
const Plane negZ( 0, 0,-1,-box.minCorner().z());
|
||||
|
||||
if (clipTo(posX) == 0) return 0;
|
||||
if (clipTo(negX) == 0) return 0;
|
||||
if (clipTo(posY) == 0) return 0;
|
||||
if (clipTo(negY) == 0) return 0;
|
||||
if (clipTo(posZ) == 0) return 0;
|
||||
if (clipTo(negZ) == 0) return 0;
|
||||
|
||||
return pointArray.count();
|
||||
}
|
||||
|
||||
|
||||
/// Clip polygon to plane.
|
||||
uint Polygon::clipTo(const Plane & plane)
|
||||
{
|
||||
int count = 0;
|
||||
|
||||
const uint pointCount = pointArray.count();
|
||||
|
||||
Array<Vector3> newPointArray(pointCount + 1); // @@ Do not create copy every time.
|
||||
|
||||
Vector3 prevPoint = pointArray[pointCount - 1];
|
||||
float prevDist = dot(plane.vector(), prevPoint) - plane.offset();
|
||||
|
||||
for (uint i = 0; i < pointCount; i++)
|
||||
{
|
||||
const Vector3 point = pointArray[i];
|
||||
float dist = dot(plane.vector(), point) - plane.offset();
|
||||
|
||||
// @@ Handle points on plane better.
|
||||
|
||||
if (dist <= 0) // interior.
|
||||
{
|
||||
if (prevDist > 0) // exterior
|
||||
{
|
||||
// Add segment intersection point.
|
||||
Vector3 dp = point - prevPoint;
|
||||
|
||||
float t = dist / prevDist;
|
||||
newPointArray.append(point - dp * t);
|
||||
}
|
||||
|
||||
// Add interior point.
|
||||
newPointArray.append(point);
|
||||
}
|
||||
else if (dist > 0 && prevDist < 0)
|
||||
{
|
||||
// Add segment intersection point.
|
||||
Vector3 dp = point - prevPoint;
|
||||
|
||||
float t = dist / prevDist;
|
||||
newPointArray.append(point - dp * t);
|
||||
}
|
||||
|
||||
prevPoint = point;
|
||||
prevDist = dist;
|
||||
}
|
||||
|
||||
swap(pointArray, newPointArray);
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
|
||||
void Polygon::removeColinearPoints()
|
||||
{
|
||||
const uint pointCount = pointArray.count();
|
||||
|
||||
Array<Vector3> newPointArray(pointCount);
|
||||
|
||||
for (uint i = 0 ; i < pointCount; i++)
|
||||
{
|
||||
int j = (i + 1) % pointCount;
|
||||
int k = (i + pointCount - 1) % pointCount;
|
||||
|
||||
Vector3 v1 = normalize(pointArray[j] - pointArray[i]);
|
||||
Vector3 v2 = normalize(pointArray[i] - pointArray[k]);
|
||||
|
||||
if (dot(v1, v2) < 0.999)
|
||||
{
|
||||
newPointArray.append(pointArray[i]);
|
||||
}
|
||||
}
|
||||
|
||||
swap(pointArray, newPointArray);
|
||||
}
|
||||
|
||||
@ -1,45 +0,0 @@
|
||||
// This code is in the public domain -- Ignacio Casta<74>o <castanyo@yahoo.es>
|
||||
|
||||
#ifndef NV_MATH_POLYGON_H
|
||||
#define NV_MATH_POLYGON_H
|
||||
|
||||
#include <nvcore/Containers.h>
|
||||
|
||||
#include <nvmath/nvmath.h>
|
||||
#include <nvmath/Vector.h>
|
||||
#include <nvmath/Box.h>
|
||||
|
||||
namespace nv
|
||||
{
|
||||
class Box;
|
||||
class Plane;
|
||||
class Triangle;
|
||||
|
||||
|
||||
class Polygon
|
||||
{
|
||||
NV_FORBID_COPY(Polygon);
|
||||
public:
|
||||
|
||||
Polygon();
|
||||
Polygon(const Triangle & t);
|
||||
Polygon(const Vector3 * points, uint vertexCount);
|
||||
|
||||
float area() const;
|
||||
Box bounds() const;
|
||||
Plane plane() const;
|
||||
|
||||
uint clipTo(const Box & box);
|
||||
uint clipTo(const Plane & plane);
|
||||
|
||||
void removeColinearPoints();
|
||||
|
||||
private:
|
||||
|
||||
Array<Vector3> pointArray;
|
||||
};
|
||||
|
||||
|
||||
} // nv namespace
|
||||
|
||||
#endif // NV_MATH_POLYGON_H
|
||||
@ -1,179 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_QUATERNION_H
|
||||
#define NV_MATH_QUATERNION_H
|
||||
|
||||
#include <nvmath/nvmath.h>
|
||||
#include <nvmath/Vector.h>
|
||||
|
||||
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)
|
||||
{
|
||||
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 mul(Quaternion::Arg a, Vector3::Arg b)
|
||||
{
|
||||
return Quaternion(
|
||||
+ a.y()*b.z() - a.z()*b.y() + a.w()*b.x(),
|
||||
- a.x() * b.z() + a.z()*b.x() + a.w()*b.y(),
|
||||
+ a.x() * b.y() - a.y()*b.x() + a.w()*b.z(),
|
||||
- a.x() * b.x() - a.y()*b.y() - a.z()*b.z() );
|
||||
}
|
||||
|
||||
inline Quaternion mul(Vector3::Arg a, Quaternion::Arg b)
|
||||
{
|
||||
return Quaternion(
|
||||
+ a.x() * b.w() + a.y()*b.z() - a.z()*b.y(),
|
||||
- a.x() * b.z() + a.y()*b.w() + a.z()*b.x(),
|
||||
+ a.x() * b.y() - a.y()*b.x() + a.z()*b.w(),
|
||||
- a.x() * b.x() - a.y()*b.y() - a.z()*b.z());
|
||||
}
|
||||
|
||||
inline Quaternion operator *(Quaternion::Arg a, Quaternion::Arg b)
|
||||
{
|
||||
return mul(a, b);
|
||||
}
|
||||
|
||||
inline Quaternion operator *(Quaternion::Arg a, Vector3::Arg b)
|
||||
{
|
||||
return mul(a, b);
|
||||
}
|
||||
|
||||
inline Quaternion operator *(Vector3::Arg a, Quaternion::Arg b)
|
||||
{
|
||||
return mul(a, b);
|
||||
}
|
||||
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
inline Vector3 imag(Quaternion::Arg q)
|
||||
{
|
||||
return q.asVector().xyz();
|
||||
}
|
||||
|
||||
inline float real(Quaternion::Arg q)
|
||||
{
|
||||
return q.w();
|
||||
}
|
||||
|
||||
|
||||
/// Transform vector.
|
||||
inline Vector3 transform(Quaternion::Arg q, Vector3::Arg v)
|
||||
{
|
||||
Quaternion t = q * v * conjugate(q);
|
||||
return imag(t);
|
||||
}
|
||||
|
||||
|
||||
} // nv namespace
|
||||
|
||||
#endif // NV_MATH_QUATERNION_H
|
||||
@ -1,739 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#include <nvmath/Solver.h>
|
||||
|
||||
using namespace nv;
|
||||
|
||||
namespace
|
||||
{
|
||||
class Preconditioner
|
||||
{
|
||||
public:
|
||||
// Virtual dtor.
|
||||
virtual ~Preconditioner() { }
|
||||
|
||||
// Apply preconditioning step.
|
||||
virtual void apply(const FullVector & x, FullVector & y) const = 0;
|
||||
};
|
||||
|
||||
|
||||
// Jacobi preconditioner.
|
||||
class JacobiPreconditioner : public Preconditioner
|
||||
{
|
||||
public:
|
||||
|
||||
JacobiPreconditioner(const SparseMatrix & M, bool symmetric) : m_inverseDiagonal(M.width())
|
||||
{
|
||||
nvCheck(M.isSquare());
|
||||
|
||||
for(uint x = 0; x < M.width(); x++)
|
||||
{
|
||||
float elem = M.getCoefficient(x, x);
|
||||
nvDebugCheck( elem != 0.0f );
|
||||
|
||||
if (symmetric)
|
||||
{
|
||||
m_inverseDiagonal[x] = 1.0f / sqrt(fabs(elem));
|
||||
}
|
||||
else
|
||||
{
|
||||
m_inverseDiagonal[x] = 1.0f / elem;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void apply(const FullVector & x, FullVector & y) const
|
||||
{
|
||||
nvDebugCheck(x.dimension() == m_inverseDiagonal.dimension());
|
||||
nvDebugCheck(y.dimension() == m_inverseDiagonal.dimension());
|
||||
|
||||
// @@ Wrap vector component-wise product into a separate function.
|
||||
const uint D = x.dimension();
|
||||
for (uint i = 0; i < D; i++)
|
||||
{
|
||||
y[i] = m_inverseDiagonal[i] * x[i];
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
FullVector m_inverseDiagonal;
|
||||
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
|
||||
static int ConjugateGradientSolver(const SparseMatrix & A, const FullVector & b, FullVector & x, float epsilon);
|
||||
static int ConjugateGradientSolver(const Preconditioner & preconditioner, const SparseMatrix & A, const FullVector & b, FullVector & x, float epsilon);
|
||||
|
||||
|
||||
// Solve the symmetric system: At<41>A<EFBFBD>x = At<41>b
|
||||
void nv::LeastSquaresSolver(const SparseMatrix & A, const FullVector & b, FullVector & x, float epsilon/*1e-5f*/)
|
||||
{
|
||||
nvDebugCheck(A.width() == x.dimension());
|
||||
nvDebugCheck(A.height() == b.dimension());
|
||||
nvDebugCheck(A.height() >= A.width()); // @@ If height == width we could solve it directly...
|
||||
|
||||
const uint D = A.width();
|
||||
|
||||
SparseMatrix At(A.height(), A.width());
|
||||
transpose(A, At);
|
||||
|
||||
FullVector Atb(D);
|
||||
//mult(Transposed, A, b, Atb);
|
||||
mult(At, b, Atb);
|
||||
|
||||
SparseMatrix AtA(D);
|
||||
//mult(Transposed, A, NoTransposed, A, AtA);
|
||||
mult(At, A, AtA);
|
||||
|
||||
SymmetricSolver(AtA, Atb, x, epsilon);
|
||||
}
|
||||
|
||||
|
||||
// See section 10.4.3 in: Mesh Parameterization: Theory and Practice, Siggraph Course Notes, August 2007
|
||||
void nv::LeastSquaresSolver(const SparseMatrix & A, const FullVector & b, FullVector & x, const uint * lockedParameters, uint lockedCount, float epsilon/*= 1e-5f*/)
|
||||
{
|
||||
nvDebugCheck(A.width() == x.dimension());
|
||||
nvDebugCheck(A.height() == b.dimension());
|
||||
nvDebugCheck(A.height() >= A.width() - lockedCount);
|
||||
|
||||
// @@ This is not the most efficient way of building a system with reduced degrees of freedom. It would be faster to do it on the fly.
|
||||
|
||||
const uint D = A.width() - lockedCount;
|
||||
nvDebugCheck(D > 0);
|
||||
|
||||
// Compute: b - Al * xl
|
||||
FullVector b_Alxl(b);
|
||||
|
||||
for (uint y = 0; y < A.height(); y++)
|
||||
{
|
||||
const uint count = A.getRow(y).count();
|
||||
for (uint e = 0; e < count; e++)
|
||||
{
|
||||
uint column = A.getRow(y)[e].x;
|
||||
|
||||
bool isFree = true;
|
||||
for (uint i = 0; i < lockedCount; i++)
|
||||
{
|
||||
isFree &= (lockedParameters[i] != column);
|
||||
}
|
||||
|
||||
if (!isFree)
|
||||
{
|
||||
b_Alxl[y] -= x[column] * A.getRow(y)[e].v;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Remove locked columns from A.
|
||||
SparseMatrix Af(D, A.height());
|
||||
|
||||
for (uint y = 0; y < A.height(); y++)
|
||||
{
|
||||
const uint count = A.getRow(y).count();
|
||||
for (uint e = 0; e < count; e++)
|
||||
{
|
||||
uint column = A.getRow(y)[e].x;
|
||||
uint ix = column;
|
||||
|
||||
bool isFree = true;
|
||||
for (uint i = 0; i < lockedCount; i++)
|
||||
{
|
||||
isFree &= (lockedParameters[i] != column);
|
||||
if (column > lockedParameters[i]) ix--; // shift columns
|
||||
}
|
||||
|
||||
if (isFree)
|
||||
{
|
||||
Af.setCoefficient(ix, y, A.getRow(y)[e].v);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Remove elements from x
|
||||
FullVector xf(D);
|
||||
|
||||
for (uint i = 0, j = 0; i < A.width(); i++)
|
||||
{
|
||||
bool isFree = true;
|
||||
for (uint l = 0; l < lockedCount; l++)
|
||||
{
|
||||
isFree &= (lockedParameters[l] != i);
|
||||
}
|
||||
|
||||
if (isFree)
|
||||
{
|
||||
xf[j++] = x[i];
|
||||
}
|
||||
}
|
||||
|
||||
// Solve reduced system.
|
||||
LeastSquaresSolver(Af, b_Alxl, xf, epsilon);
|
||||
|
||||
// Copy results back to x.
|
||||
for (uint i = 0, j = 0; i < A.width(); i++)
|
||||
{
|
||||
bool isFree = true;
|
||||
for (uint l = 0; l < lockedCount; l++)
|
||||
{
|
||||
isFree &= (lockedParameters[l] != i);
|
||||
}
|
||||
|
||||
if (isFree)
|
||||
{
|
||||
x[i] = xf[j++];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void nv::SymmetricSolver(const SparseMatrix & A, const FullVector & b, FullVector & x, float epsilon/*1e-5f*/)
|
||||
{
|
||||
nvDebugCheck(A.height() == A.width());
|
||||
nvDebugCheck(A.height() == b.dimension());
|
||||
nvDebugCheck(b.dimension() == x.dimension());
|
||||
|
||||
JacobiPreconditioner jacobi(A, true);
|
||||
ConjugateGradientSolver(jacobi, A, b, x, epsilon);
|
||||
|
||||
// ConjugateGradientSolver(A, b, x, epsilon);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Compute the solution of the sparse linear system Ab=x using the Conjugate
|
||||
* Gradient method.
|
||||
*
|
||||
* Solving sparse linear systems:
|
||||
* (1) A<>x = b
|
||||
*
|
||||
* The conjugate gradient algorithm solves (1) only in the case that A is
|
||||
* symmetric and positive definite. It is based on the idea of minimizing the
|
||||
* function
|
||||
*
|
||||
* (2) f(x) = 1/2<>x<EFBFBD>A<EFBFBD>x - b<>x
|
||||
*
|
||||
* This function is minimized when its gradient
|
||||
*
|
||||
* (3) df = A<>x - b
|
||||
*
|
||||
* is zero, which is equivalent to (1). The minimization is carried out by
|
||||
* generating a succession of search directions p.k and improved minimizers x.k.
|
||||
* At each stage a quantity alfa.k is found that minimizes f(x.k + alfa.k<>p.k),
|
||||
* and x.k+1 is set equal to the new point x.k + alfa.k<>p.k. The p.k and x.k are
|
||||
* built up in such a way that x.k+1 is also the minimizer of f over the whole
|
||||
* vector space of directions already taken, {p.1, p.2, . . . , p.k}. After N
|
||||
* iterations you arrive at the minimizer over the entire vector space, i.e., the
|
||||
* solution to (1).
|
||||
*
|
||||
* For a really good explanation of the method see:
|
||||
*
|
||||
* "An Introduction to the Conjugate Gradient Method Without the Agonizing Pain",
|
||||
* Jonhathan Richard Shewchuk.
|
||||
*
|
||||
**/
|
||||
/*static*/ int ConjugateGradientSolver(const SparseMatrix & A, const FullVector & b, FullVector & x, float epsilon)
|
||||
{
|
||||
nvDebugCheck( A.isSquare() );
|
||||
nvDebugCheck( A.width() == b.dimension() );
|
||||
nvDebugCheck( A.width() == x.dimension() );
|
||||
|
||||
int i = 0;
|
||||
const int D = A.width();
|
||||
const int i_max = 4 * D; // Convergence should be linear, but in some cases, it's not.
|
||||
|
||||
FullVector r(D); // residual
|
||||
FullVector p(D); // search direction
|
||||
FullVector q(D); //
|
||||
float delta_0;
|
||||
float delta_old;
|
||||
float delta_new;
|
||||
float alpha;
|
||||
float beta;
|
||||
|
||||
// r = b - A<>x;
|
||||
copy(b, r);
|
||||
sgemv(-1, A, x, 1, r);
|
||||
|
||||
// p = r;
|
||||
copy(r, p);
|
||||
|
||||
delta_new = dot( r, r );
|
||||
delta_0 = delta_new;
|
||||
|
||||
while (i < i_max && delta_new > epsilon*epsilon*delta_0)
|
||||
{
|
||||
i++;
|
||||
|
||||
// q = A<>p
|
||||
mult(A, p, q);
|
||||
|
||||
// alpha = delta_new / p<>q
|
||||
alpha = delta_new / dot( p, q );
|
||||
|
||||
// x = alfa<66>p + x
|
||||
saxpy(alpha, p, x);
|
||||
|
||||
if ((i & 31) == 0) // recompute r after 32 steps
|
||||
{
|
||||
// r = b - A<>x
|
||||
copy(b, r);
|
||||
sgemv(-1, A, x, 1, r);
|
||||
}
|
||||
else
|
||||
{
|
||||
// r = r - alpha<68>q
|
||||
saxpy(-alpha, q, r);
|
||||
}
|
||||
|
||||
delta_old = delta_new;
|
||||
delta_new = dot( r, r );
|
||||
|
||||
beta = delta_new / delta_old;
|
||||
|
||||
// p = beta<74>p + r
|
||||
scal(beta, p);
|
||||
saxpy(1, r, p);
|
||||
}
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
|
||||
// Conjugate gradient with preconditioner.
|
||||
/*static*/ int ConjugateGradientSolver(const Preconditioner & preconditioner, const SparseMatrix & A, const FullVector & b, FullVector & x, float epsilon)
|
||||
{
|
||||
nvDebugCheck( A.isSquare() );
|
||||
nvDebugCheck( A.width() == b.dimension() );
|
||||
nvDebugCheck( A.width() == x.dimension() );
|
||||
|
||||
int i = 0;
|
||||
const int D = A.width();
|
||||
const int i_max = 4 * D; // Convergence should be linear, but in some cases, it's not.
|
||||
|
||||
FullVector r(D); // residual
|
||||
FullVector p(D); // search direction
|
||||
FullVector q(D); //
|
||||
FullVector s(D); // preconditioned
|
||||
float delta_0;
|
||||
float delta_old;
|
||||
float delta_new;
|
||||
float alpha;
|
||||
float beta;
|
||||
|
||||
// r = b - A<>x
|
||||
copy(b, r);
|
||||
sgemv(-1, A, x, 1, r);
|
||||
|
||||
|
||||
// p = M^-1 <20> r
|
||||
preconditioner.apply(r, p);
|
||||
//copy(r, p);
|
||||
|
||||
|
||||
delta_new = dot(r, p);
|
||||
delta_0 = delta_new;
|
||||
|
||||
while (i < i_max && delta_new > epsilon*epsilon*delta_0)
|
||||
{
|
||||
i++;
|
||||
|
||||
// q = A<>p
|
||||
mult(A, p, q);
|
||||
|
||||
// alpha = delta_new / p<>q
|
||||
alpha = delta_new / dot(p, q);
|
||||
|
||||
// x = alfa<66>p + x
|
||||
saxpy(alpha, p, x);
|
||||
|
||||
if ((i & 31) == 0) // recompute r after 32 steps
|
||||
{
|
||||
// r = b - A<>x
|
||||
copy(b, r);
|
||||
sgemv(-1, A, x, 1, r);
|
||||
}
|
||||
else
|
||||
{
|
||||
// r = r - alfa<66>q
|
||||
saxpy(-alpha, q, r);
|
||||
}
|
||||
|
||||
// s = M^-1 <20> r
|
||||
preconditioner.apply(r, s);
|
||||
//copy(r, s);
|
||||
|
||||
delta_old = delta_new;
|
||||
delta_new = dot( r, s );
|
||||
|
||||
beta = delta_new / delta_old;
|
||||
|
||||
// p = s + beta<74>p
|
||||
scal(beta, p);
|
||||
saxpy(1, s, p);
|
||||
}
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
|
||||
#if 0 // Nonsymmetric solvers
|
||||
|
||||
/** Bi-conjugate gradient method. */
|
||||
MATHLIB_API int BiConjugateGradientSolve( const SparseMatrix &A, const DenseVector &b, DenseVector &x, float epsilon ) {
|
||||
piDebugCheck( A.IsSquare() );
|
||||
piDebugCheck( A.Width() == b.Dim() );
|
||||
piDebugCheck( A.Width() == x.Dim() );
|
||||
|
||||
int i = 0;
|
||||
const int D = A.Width();
|
||||
const int i_max = 4 * D;
|
||||
|
||||
float resid;
|
||||
float rho_1 = 0;
|
||||
float rho_2 = 0;
|
||||
float alpha;
|
||||
float beta;
|
||||
|
||||
DenseVector r(D);
|
||||
DenseVector rtilde(D);
|
||||
DenseVector p(D);
|
||||
DenseVector ptilde(D);
|
||||
DenseVector q(D);
|
||||
DenseVector qtilde(D);
|
||||
DenseVector tmp(D); // temporal vector.
|
||||
|
||||
// r = b - A<>x;
|
||||
A.Product( x, tmp );
|
||||
r.Sub( b, tmp );
|
||||
|
||||
// rtilde = r
|
||||
rtilde.Set( r );
|
||||
|
||||
// p = r;
|
||||
p.Set( r );
|
||||
|
||||
// ptilde = rtilde
|
||||
ptilde.Set( rtilde );
|
||||
|
||||
|
||||
|
||||
float normb = b.Norm();
|
||||
if( normb == 0.0 ) normb = 1;
|
||||
|
||||
// test convergence
|
||||
resid = r.Norm() / normb;
|
||||
if( resid < epsilon ) {
|
||||
// method converges?
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
while( i < i_max ) {
|
||||
|
||||
i++;
|
||||
|
||||
rho_1 = DenseVectorDotProduct( r, rtilde );
|
||||
|
||||
if( rho_1 == 0 ) {
|
||||
// method fails.
|
||||
return -i;
|
||||
}
|
||||
|
||||
if (i == 1) {
|
||||
p.Set( r );
|
||||
ptilde.Set( rtilde );
|
||||
}
|
||||
else {
|
||||
beta = rho_1 / rho_2;
|
||||
|
||||
// p = r + beta * p;
|
||||
p.Mad( r, p, beta );
|
||||
|
||||
// ptilde = ztilde + beta * ptilde;
|
||||
ptilde.Mad( rtilde, ptilde, beta );
|
||||
}
|
||||
|
||||
// q = A * p;
|
||||
A.Product( p, q );
|
||||
|
||||
// qtilde = A^t * ptilde;
|
||||
A.TransProduct( ptilde, qtilde );
|
||||
|
||||
alpha = rho_1 / DenseVectorDotProduct( ptilde, q );
|
||||
|
||||
// x += alpha * p;
|
||||
x.Mad( x, p, alpha );
|
||||
|
||||
// r -= alpha * q;
|
||||
r.Mad( r, q, -alpha );
|
||||
|
||||
// rtilde -= alpha * qtilde;
|
||||
rtilde.Mad( rtilde, qtilde, -alpha );
|
||||
|
||||
rho_2 = rho_1;
|
||||
|
||||
// test convergence
|
||||
resid = r.Norm() / normb;
|
||||
if( resid < epsilon ) {
|
||||
// method converges
|
||||
return i;
|
||||
}
|
||||
}
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
|
||||
/** Bi-conjugate gradient stabilized method. */
|
||||
int BiCGSTABSolve( const SparseMatrix &A, const DenseVector &b, DenseVector &x, float epsilon ) {
|
||||
piDebugCheck( A.IsSquare() );
|
||||
piDebugCheck( A.Width() == b.Dim() );
|
||||
piDebugCheck( A.Width() == x.Dim() );
|
||||
|
||||
int i = 0;
|
||||
const int D = A.Width();
|
||||
const int i_max = 2 * D;
|
||||
|
||||
|
||||
float resid;
|
||||
float rho_1 = 0;
|
||||
float rho_2 = 0;
|
||||
float alpha = 0;
|
||||
float beta = 0;
|
||||
float omega = 0;
|
||||
|
||||
DenseVector p(D);
|
||||
DenseVector phat(D);
|
||||
DenseVector s(D);
|
||||
DenseVector shat(D);
|
||||
DenseVector t(D);
|
||||
DenseVector v(D);
|
||||
|
||||
DenseVector r(D);
|
||||
DenseVector rtilde(D);
|
||||
|
||||
DenseVector tmp(D);
|
||||
|
||||
// r = b - A<>x;
|
||||
A.Product( x, tmp );
|
||||
r.Sub( b, tmp );
|
||||
|
||||
// rtilde = r
|
||||
rtilde.Set( r );
|
||||
|
||||
|
||||
float normb = b.Norm();
|
||||
if( normb == 0.0 ) normb = 1;
|
||||
|
||||
// test convergence
|
||||
resid = r.Norm() / normb;
|
||||
if( resid < epsilon ) {
|
||||
// method converges?
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
while( i<i_max ) {
|
||||
|
||||
i++;
|
||||
|
||||
rho_1 = DenseVectorDotProduct( rtilde, r );
|
||||
if( rho_1 == 0 ) {
|
||||
// method fails
|
||||
return -i;
|
||||
}
|
||||
|
||||
|
||||
if( i == 1 ) {
|
||||
p.Set( r );
|
||||
}
|
||||
else {
|
||||
beta = (rho_1 / rho_2) * (alpha / omega);
|
||||
|
||||
// p = r + beta * (p - omega * v);
|
||||
p.Mad( p, v, -omega );
|
||||
p.Mad( r, p, beta );
|
||||
}
|
||||
|
||||
//phat = M.solve(p);
|
||||
phat.Set( p );
|
||||
//Precond( &phat, p );
|
||||
|
||||
//v = A * phat;
|
||||
A.Product( phat, v );
|
||||
|
||||
alpha = rho_1 / DenseVectorDotProduct( rtilde, v );
|
||||
|
||||
// s = r - alpha * v;
|
||||
s.Mad( r, v, -alpha );
|
||||
|
||||
|
||||
resid = s.Norm() / normb;
|
||||
if( resid < epsilon ) {
|
||||
// x += alpha * phat;
|
||||
x.Mad( x, phat, alpha );
|
||||
return i;
|
||||
}
|
||||
|
||||
//shat = M.solve(s);
|
||||
shat.Set( s );
|
||||
//Precond( &shat, s );
|
||||
|
||||
//t = A * shat;
|
||||
A.Product( shat, t );
|
||||
|
||||
omega = DenseVectorDotProduct( t, s ) / DenseVectorDotProduct( t, t );
|
||||
|
||||
// x += alpha * phat + omega * shat;
|
||||
x.Mad( x, shat, omega );
|
||||
x.Mad( x, phat, alpha );
|
||||
|
||||
//r = s - omega * t;
|
||||
r.Mad( s, t, -omega );
|
||||
|
||||
rho_2 = rho_1;
|
||||
|
||||
resid = r.Norm() / normb;
|
||||
if( resid < epsilon ) {
|
||||
return i;
|
||||
}
|
||||
|
||||
if( omega == 0 ) {
|
||||
return -i; // ???
|
||||
}
|
||||
}
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
|
||||
/** Bi-conjugate gradient stabilized method. */
|
||||
int BiCGSTABPrecondSolve( const SparseMatrix &A, const DenseVector &b, DenseVector &x, const IPreconditioner &M, float epsilon ) {
|
||||
piDebugCheck( A.IsSquare() );
|
||||
piDebugCheck( A.Width() == b.Dim() );
|
||||
piDebugCheck( A.Width() == x.Dim() );
|
||||
|
||||
int i = 0;
|
||||
const int D = A.Width();
|
||||
const int i_max = D;
|
||||
// const int i_max = 1000;
|
||||
|
||||
|
||||
float resid;
|
||||
float rho_1 = 0;
|
||||
float rho_2 = 0;
|
||||
float alpha = 0;
|
||||
float beta = 0;
|
||||
float omega = 0;
|
||||
|
||||
DenseVector p(D);
|
||||
DenseVector phat(D);
|
||||
DenseVector s(D);
|
||||
DenseVector shat(D);
|
||||
DenseVector t(D);
|
||||
DenseVector v(D);
|
||||
|
||||
DenseVector r(D);
|
||||
DenseVector rtilde(D);
|
||||
|
||||
DenseVector tmp(D);
|
||||
|
||||
// r = b - A<>x;
|
||||
A.Product( x, tmp );
|
||||
r.Sub( b, tmp );
|
||||
|
||||
// rtilde = r
|
||||
rtilde.Set( r );
|
||||
|
||||
|
||||
float normb = b.Norm();
|
||||
if( normb == 0.0 ) normb = 1;
|
||||
|
||||
// test convergence
|
||||
resid = r.Norm() / normb;
|
||||
if( resid < epsilon ) {
|
||||
// method converges?
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
while( i<i_max ) {
|
||||
|
||||
i++;
|
||||
|
||||
rho_1 = DenseVectorDotProduct( rtilde, r );
|
||||
if( rho_1 == 0 ) {
|
||||
// method fails
|
||||
return -i;
|
||||
}
|
||||
|
||||
|
||||
if( i == 1 ) {
|
||||
p.Set( r );
|
||||
}
|
||||
else {
|
||||
beta = (rho_1 / rho_2) * (alpha / omega);
|
||||
|
||||
// p = r + beta * (p - omega * v);
|
||||
p.Mad( p, v, -omega );
|
||||
p.Mad( r, p, beta );
|
||||
}
|
||||
|
||||
//phat = M.solve(p);
|
||||
//phat.Set( p );
|
||||
M.Precond( &phat, p );
|
||||
|
||||
//v = A * phat;
|
||||
A.Product( phat, v );
|
||||
|
||||
alpha = rho_1 / DenseVectorDotProduct( rtilde, v );
|
||||
|
||||
// s = r - alpha * v;
|
||||
s.Mad( r, v, -alpha );
|
||||
|
||||
|
||||
resid = s.Norm() / normb;
|
||||
|
||||
//printf( "--- Iteration %d: residual = %f\n", i, resid );
|
||||
|
||||
if( resid < epsilon ) {
|
||||
// x += alpha * phat;
|
||||
x.Mad( x, phat, alpha );
|
||||
return i;
|
||||
}
|
||||
|
||||
//shat = M.solve(s);
|
||||
//shat.Set( s );
|
||||
M.Precond( &shat, s );
|
||||
|
||||
//t = A * shat;
|
||||
A.Product( shat, t );
|
||||
|
||||
omega = DenseVectorDotProduct( t, s ) / DenseVectorDotProduct( t, t );
|
||||
|
||||
// x += alpha * phat + omega * shat;
|
||||
x.Mad( x, shat, omega );
|
||||
x.Mad( x, phat, alpha );
|
||||
|
||||
//r = s - omega * t;
|
||||
r.Mad( s, t, -omega );
|
||||
|
||||
rho_2 = rho_1;
|
||||
|
||||
resid = r.Norm() / normb;
|
||||
if( resid < epsilon ) {
|
||||
return i;
|
||||
}
|
||||
|
||||
if( omega == 0 ) {
|
||||
return -i; // ???
|
||||
}
|
||||
}
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -1,20 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_SOLVER_H
|
||||
#define NV_MATH_SOLVER_H
|
||||
|
||||
#include <nvmath/Sparse.h>
|
||||
|
||||
namespace nv
|
||||
{
|
||||
|
||||
// Linear solvers.
|
||||
NVMATH_API void LeastSquaresSolver(const SparseMatrix & A, const FullVector & b, FullVector & x, float epsilon = 1e-5f);
|
||||
NVMATH_API void LeastSquaresSolver(const SparseMatrix & A, const FullVector & b, FullVector & x, const uint * lockedParameters, uint lockedCount, float epsilon = 1e-5f);
|
||||
NVMATH_API void SymmetricSolver(const SparseMatrix & A, const FullVector & b, FullVector & x, float epsilon = 1e-5f);
|
||||
// NVMATH_API void NonSymmetricSolver(const SparseMatrix & A, const FullVector & b, FullVector & x, float epsilon = 1e-5f);
|
||||
|
||||
} // nv namespace
|
||||
|
||||
|
||||
#endif // NV_MATH_SOLVER_H
|
||||
@ -1,862 +0,0 @@
|
||||
// This code is in the public domain -- Ignacio Casta<74>o <castanyo@yahoo.es>
|
||||
|
||||
#include <nvmath/Sparse.h>
|
||||
#include <nvmath/KahanSum.h>
|
||||
|
||||
using namespace nv;
|
||||
|
||||
|
||||
/// Ctor.
|
||||
FullVector::FullVector(uint dim)
|
||||
{
|
||||
m_array.resize(dim);
|
||||
}
|
||||
|
||||
/// Copy ctor.
|
||||
FullVector::FullVector(const FullVector & v) : m_array(v.m_array)
|
||||
{
|
||||
}
|
||||
|
||||
/// Copy operator
|
||||
const FullVector & FullVector::operator=(const FullVector & v)
|
||||
{
|
||||
nvCheck(dimension() == v.dimension());
|
||||
|
||||
m_array = v.m_array;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
void FullVector::fill(float f)
|
||||
{
|
||||
const uint dim = dimension();
|
||||
for (uint i = 0; i < dim; i++)
|
||||
{
|
||||
m_array[i] = f;
|
||||
}
|
||||
}
|
||||
|
||||
void FullVector::operator+= (const FullVector & v)
|
||||
{
|
||||
nvDebugCheck(dimension() == v.dimension());
|
||||
|
||||
const uint dim = dimension();
|
||||
for (uint i = 0; i < dim; i++)
|
||||
{
|
||||
m_array[i] += v.m_array[i];
|
||||
}
|
||||
}
|
||||
|
||||
void FullVector::operator-= (const FullVector & v)
|
||||
{
|
||||
nvDebugCheck(dimension() == v.dimension());
|
||||
|
||||
const uint dim = dimension();
|
||||
for (uint i = 0; i < dim; i++)
|
||||
{
|
||||
m_array[i] -= v.m_array[i];
|
||||
}
|
||||
}
|
||||
|
||||
void FullVector::operator*= (const FullVector & v)
|
||||
{
|
||||
nvDebugCheck(dimension() == v.dimension());
|
||||
|
||||
const uint dim = dimension();
|
||||
for (uint i = 0; i < dim; i++)
|
||||
{
|
||||
m_array[i] *= v.m_array[i];
|
||||
}
|
||||
}
|
||||
|
||||
void FullVector::operator+= (float f)
|
||||
{
|
||||
const uint dim = dimension();
|
||||
for (uint i = 0; i < dim; i++)
|
||||
{
|
||||
m_array[i] += f;
|
||||
}
|
||||
}
|
||||
|
||||
void FullVector::operator-= (float f)
|
||||
{
|
||||
const uint dim = dimension();
|
||||
for (uint i = 0; i < dim; i++)
|
||||
{
|
||||
m_array[i] -= f;
|
||||
}
|
||||
}
|
||||
|
||||
void FullVector::operator*= (float f)
|
||||
{
|
||||
const uint dim = dimension();
|
||||
for (uint i = 0; i < dim; i++)
|
||||
{
|
||||
m_array[i] *= f;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void nv::saxpy(float a, const FullVector & x, FullVector & y)
|
||||
{
|
||||
nvDebugCheck(x.dimension() == y.dimension());
|
||||
|
||||
const uint dim = x.dimension();
|
||||
for (uint i = 0; i < dim; i++)
|
||||
{
|
||||
y[i] += a * x[i];
|
||||
}
|
||||
}
|
||||
|
||||
void nv::copy(const FullVector & x, FullVector & y)
|
||||
{
|
||||
nvDebugCheck(x.dimension() == y.dimension());
|
||||
|
||||
const uint dim = x.dimension();
|
||||
for (uint i = 0; i < dim; i++)
|
||||
{
|
||||
y[i] = x[i];
|
||||
}
|
||||
}
|
||||
|
||||
void nv::scal(float a, FullVector & x)
|
||||
{
|
||||
const uint dim = x.dimension();
|
||||
for (uint i = 0; i < dim; i++)
|
||||
{
|
||||
x[i] *= a;
|
||||
}
|
||||
}
|
||||
|
||||
float nv::dot(const FullVector & x, const FullVector & y)
|
||||
{
|
||||
nvDebugCheck(x.dimension() == y.dimension());
|
||||
|
||||
const uint dim = x.dimension();
|
||||
|
||||
/*float sum = 0;
|
||||
for (uint i = 0; i < dim; i++)
|
||||
{
|
||||
sum += x[i] * y[i];
|
||||
}
|
||||
return sum;*/
|
||||
|
||||
KahanSum kahan;
|
||||
|
||||
for (uint i = 0; i < dim; i++)
|
||||
{
|
||||
kahan.add(x[i] * y[i]);
|
||||
}
|
||||
|
||||
return kahan.sum();
|
||||
}
|
||||
|
||||
|
||||
FullMatrix::FullMatrix(uint d) : m_width(d), m_height(d)
|
||||
{
|
||||
m_array.resize(d*d, 0.0f);
|
||||
}
|
||||
|
||||
FullMatrix::FullMatrix(uint w, uint h) : m_width(w), m_height(h)
|
||||
{
|
||||
m_array.resize(w*h, 0.0f);
|
||||
}
|
||||
|
||||
FullMatrix::FullMatrix(const FullMatrix & m) : m_width(m.m_width), m_height(m.m_height)
|
||||
{
|
||||
m_array = m.m_array;
|
||||
}
|
||||
|
||||
const FullMatrix & FullMatrix::operator=(const FullMatrix & m)
|
||||
{
|
||||
nvCheck(width() == m.width());
|
||||
nvCheck(height() == m.height());
|
||||
|
||||
m_array = m.m_array;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
float FullMatrix::getCoefficient(uint x, uint y) const
|
||||
{
|
||||
nvDebugCheck( x < width() );
|
||||
nvDebugCheck( y < height() );
|
||||
|
||||
return m_array[y * width() + x];
|
||||
}
|
||||
|
||||
void FullMatrix::setCoefficient(uint x, uint y, float f)
|
||||
{
|
||||
nvDebugCheck( x < width() );
|
||||
nvDebugCheck( y < height() );
|
||||
|
||||
m_array[y * width() + x] = f;
|
||||
}
|
||||
|
||||
void FullMatrix::addCoefficient(uint x, uint y, float f)
|
||||
{
|
||||
nvDebugCheck( x < width() );
|
||||
nvDebugCheck( y < height() );
|
||||
|
||||
m_array[y * width() + x] += f;
|
||||
}
|
||||
|
||||
void FullMatrix::mulCoefficient(uint x, uint y, float f)
|
||||
{
|
||||
nvDebugCheck( x < width() );
|
||||
nvDebugCheck( y < height() );
|
||||
|
||||
m_array[y * width() + x] *= f;
|
||||
}
|
||||
|
||||
float FullMatrix::dotRow(uint y, const FullVector & v) const
|
||||
{
|
||||
nvDebugCheck( v.dimension() == width() );
|
||||
nvDebugCheck( y < height() );
|
||||
|
||||
float sum = 0;
|
||||
|
||||
const uint count = v.dimension();
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
sum += m_array[y * count + i] * v[i];
|
||||
}
|
||||
|
||||
return sum;
|
||||
}
|
||||
|
||||
void FullMatrix::madRow(uint y, float alpha, FullVector & v) const
|
||||
{
|
||||
nvDebugCheck( v.dimension() == width() );
|
||||
nvDebugCheck( y < height() );
|
||||
|
||||
const uint count = v.dimension();
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
v[i] += m_array[y * count + i];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// y = M * x
|
||||
void nv::mult(const FullMatrix & M, const FullVector & x, FullVector & y)
|
||||
{
|
||||
mult(NoTransposed, M, x, y);
|
||||
}
|
||||
|
||||
void nv::mult(Transpose TM, const FullMatrix & M, const FullVector & x, FullVector & y)
|
||||
{
|
||||
const uint w = M.width();
|
||||
const uint h = M.height();
|
||||
|
||||
if (TM == Transposed)
|
||||
{
|
||||
nvDebugCheck( h == x.dimension() );
|
||||
nvDebugCheck( w == y.dimension() );
|
||||
|
||||
y.fill(0.0f);
|
||||
|
||||
for (uint i = 0; i < h; i++)
|
||||
{
|
||||
M.madRow(i, x[i], y);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
nvDebugCheck( w == x.dimension() );
|
||||
nvDebugCheck( h == y.dimension() );
|
||||
|
||||
for (uint i = 0; i < h; i++)
|
||||
{
|
||||
y[i] = M.dotRow(i, x);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// y = alpha*A*x + beta*y
|
||||
void nv::sgemv(float alpha, const FullMatrix & A, const FullVector & x, float beta, FullVector & y)
|
||||
{
|
||||
sgemv(alpha, NoTransposed, A, x, beta, y);
|
||||
}
|
||||
|
||||
void nv::sgemv(float alpha, Transpose TA, const FullMatrix & A, const FullVector & x, float beta, FullVector & y)
|
||||
{
|
||||
const uint w = A.width();
|
||||
const uint h = A.height();
|
||||
|
||||
if (TA == Transposed)
|
||||
{
|
||||
nvDebugCheck( h == x.dimension() );
|
||||
nvDebugCheck( w == y.dimension() );
|
||||
|
||||
for (uint i = 0; i < h; i++)
|
||||
{
|
||||
A.madRow(i, alpha * x[i], y);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
nvDebugCheck( w == x.dimension() );
|
||||
nvDebugCheck( h == y.dimension() );
|
||||
|
||||
for (uint i = 0; i < h; i++)
|
||||
{
|
||||
y[i] = alpha * A.dotRow(i, x) + beta * y[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Multiply a row of A by a column of B.
|
||||
static float dot(uint j, Transpose TA, const FullMatrix & A, uint i, Transpose TB, const FullMatrix & B)
|
||||
{
|
||||
const uint w = (TA == NoTransposed) ? A.width() : A.height();
|
||||
nvDebugCheck(w == (TB == NoTransposed) ? B.height() : A.width());
|
||||
|
||||
float sum = 0.0f;
|
||||
|
||||
for (uint k = 0; k < w; k++)
|
||||
{
|
||||
const float a = (TA == NoTransposed) ? A.getCoefficient(k, j) : A.getCoefficient(j, k); // @@ Move branches out of the loop?
|
||||
const float b = (TB == NoTransposed) ? B.getCoefficient(i, k) : A.getCoefficient(k, i);
|
||||
sum += a * b;
|
||||
}
|
||||
|
||||
return sum;
|
||||
}
|
||||
|
||||
|
||||
// C = A * B
|
||||
void nv::mult(const FullMatrix & A, const FullMatrix & B, FullMatrix & C)
|
||||
{
|
||||
mult(NoTransposed, A, NoTransposed, B, C);
|
||||
}
|
||||
|
||||
void nv::mult(Transpose TA, const FullMatrix & A, Transpose TB, const FullMatrix & B, FullMatrix & C)
|
||||
{
|
||||
sgemm(1.0f, TA, A, TB, B, 0.0f, C);
|
||||
}
|
||||
|
||||
// C = alpha*A*B + beta*C
|
||||
void nv::sgemm(float alpha, const FullMatrix & A, const FullMatrix & B, float beta, FullMatrix & C)
|
||||
{
|
||||
sgemm(alpha, NoTransposed, A, NoTransposed, B, beta, C);
|
||||
}
|
||||
|
||||
void nv::sgemm(float alpha, Transpose TA, const FullMatrix & A, Transpose TB, const FullMatrix & B, float beta, FullMatrix & C)
|
||||
{
|
||||
const uint w = C.width();
|
||||
const uint h = C.height();
|
||||
|
||||
uint aw = (TA == NoTransposed) ? A.width() : A.height();
|
||||
uint ah = (TA == NoTransposed) ? A.height() : A.width();
|
||||
uint bw = (TB == NoTransposed) ? B.width() : B.height();
|
||||
uint bh = (TB == NoTransposed) ? B.height() : B.width();
|
||||
|
||||
nvDebugCheck(aw == bh);
|
||||
nvDebugCheck(bw == ah);
|
||||
nvDebugCheck(w == bw);
|
||||
nvDebugCheck(h == ah);
|
||||
|
||||
for (uint y = 0; y < h; y++)
|
||||
{
|
||||
for (uint x = 0; x < w; x++)
|
||||
{
|
||||
float c = alpha * ::dot(x, TA, A, y, TB, B) + beta * C.getCoefficient(x, y);
|
||||
C.setCoefficient(x, y, c);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/// Ctor. Init the size of the sparse matrix.
|
||||
SparseMatrix::SparseMatrix(uint d) : m_width(d)
|
||||
{
|
||||
m_array.resize(d);
|
||||
}
|
||||
|
||||
/// Ctor. Init the size of the sparse matrix.
|
||||
SparseMatrix::SparseMatrix(uint w, uint h) : m_width(w)
|
||||
{
|
||||
m_array.resize(h);
|
||||
}
|
||||
|
||||
SparseMatrix::SparseMatrix(const SparseMatrix & m) : m_width(m.m_width)
|
||||
{
|
||||
m_array = m.m_array;
|
||||
}
|
||||
|
||||
const SparseMatrix & SparseMatrix::operator=(const SparseMatrix & m)
|
||||
{
|
||||
nvCheck(width() == m.width());
|
||||
nvCheck(height() == m.height());
|
||||
|
||||
m_array = m.m_array;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
// x is column, y is row
|
||||
float SparseMatrix::getCoefficient(uint x, uint y) const
|
||||
{
|
||||
nvDebugCheck( x < width() );
|
||||
nvDebugCheck( y < height() );
|
||||
|
||||
const uint count = m_array[y].count();
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
if (m_array[y][i].x == x) return m_array[y][i].v;
|
||||
}
|
||||
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
void SparseMatrix::setCoefficient(uint x, uint y, float f)
|
||||
{
|
||||
nvDebugCheck( x < width() );
|
||||
nvDebugCheck( y < height() );
|
||||
|
||||
const uint count = m_array[y].count();
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
if (m_array[y][i].x == x)
|
||||
{
|
||||
m_array[y][i].v = f;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (f != 0.0f)
|
||||
{
|
||||
Coefficient c = { x, f };
|
||||
m_array[y].append( c );
|
||||
}
|
||||
}
|
||||
|
||||
void SparseMatrix::addCoefficient(uint x, uint y, float f)
|
||||
{
|
||||
nvDebugCheck( x < width() );
|
||||
nvDebugCheck( y < height() );
|
||||
|
||||
if (f != 0.0f)
|
||||
{
|
||||
const uint count = m_array[y].count();
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
if (m_array[y][i].x == x)
|
||||
{
|
||||
m_array[y][i].v += f;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
Coefficient c = { x, f };
|
||||
m_array[y].append( c );
|
||||
}
|
||||
}
|
||||
|
||||
void SparseMatrix::mulCoefficient(uint x, uint y, float f)
|
||||
{
|
||||
nvDebugCheck( x < width() );
|
||||
nvDebugCheck( y < height() );
|
||||
|
||||
const uint count = m_array[y].count();
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
if (m_array[y][i].x == x)
|
||||
{
|
||||
m_array[y][i].v *= f;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (f != 0.0f)
|
||||
{
|
||||
Coefficient c = { x, f };
|
||||
m_array[y].append( c );
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
float SparseMatrix::sumRow(uint y) const
|
||||
{
|
||||
nvDebugCheck( y < height() );
|
||||
|
||||
const uint count = m_array[y].count();
|
||||
|
||||
/*float sum = 0;
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
sum += m_array[y][i].v;
|
||||
}
|
||||
return sum;*/
|
||||
|
||||
KahanSum kahan;
|
||||
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
kahan.add(m_array[y][i].v);
|
||||
}
|
||||
|
||||
return kahan.sum();
|
||||
}
|
||||
|
||||
float SparseMatrix::dotRow(uint y, const FullVector & v) const
|
||||
{
|
||||
nvDebugCheck( y < height() );
|
||||
|
||||
const uint count = m_array[y].count();
|
||||
|
||||
/*float sum = 0;
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
sum += m_array[y][i].v * v[m_array[y][i].x];
|
||||
}
|
||||
return sum;*/
|
||||
|
||||
KahanSum kahan;
|
||||
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
kahan.add(m_array[y][i].v * v[m_array[y][i].x]);
|
||||
}
|
||||
|
||||
return kahan.sum();
|
||||
}
|
||||
|
||||
void SparseMatrix::madRow(uint y, float alpha, FullVector & v) const
|
||||
{
|
||||
nvDebugCheck(y < height());
|
||||
|
||||
const uint count = m_array[y].count();
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
v[m_array[y][i].x] += alpha * m_array[y][i].v;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void SparseMatrix::clearRow(uint y)
|
||||
{
|
||||
nvDebugCheck( y < height() );
|
||||
|
||||
m_array[y].clear();
|
||||
}
|
||||
|
||||
void SparseMatrix::scaleRow(uint y, float f)
|
||||
{
|
||||
nvDebugCheck( y < height() );
|
||||
|
||||
const uint count = m_array[y].count();
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
m_array[y][i].v *= f;
|
||||
}
|
||||
}
|
||||
|
||||
void SparseMatrix::normalizeRow(uint y)
|
||||
{
|
||||
nvDebugCheck( y < height() );
|
||||
|
||||
float norm = 0.0f;
|
||||
|
||||
const uint count = m_array[y].count();
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
float f = m_array[y][i].v;
|
||||
norm += f * f;
|
||||
}
|
||||
|
||||
scaleRow(y, 1.0f / sqrtf(norm));
|
||||
}
|
||||
|
||||
|
||||
void SparseMatrix::clearColumn(uint x)
|
||||
{
|
||||
nvDebugCheck(x < width());
|
||||
|
||||
for (uint y = 0; y < height(); y++)
|
||||
{
|
||||
const uint count = m_array[y].count();
|
||||
for (uint e = 0; e < count; e++)
|
||||
{
|
||||
if (m_array[y][e].x == x)
|
||||
{
|
||||
m_array[y][e].v = 0.0f;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void SparseMatrix::scaleColumn(uint x, float f)
|
||||
{
|
||||
nvDebugCheck(x < width());
|
||||
|
||||
for (uint y = 0; y < height(); y++)
|
||||
{
|
||||
const uint count = m_array[y].count();
|
||||
for (uint e = 0; e < count; e++)
|
||||
{
|
||||
if (m_array[y][e].x == x)
|
||||
{
|
||||
m_array[y][e].v *= f;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const Array<SparseMatrix::Coefficient> & SparseMatrix::getRow(uint y) const
|
||||
{
|
||||
return m_array[y];
|
||||
}
|
||||
|
||||
|
||||
// y = M * x
|
||||
void nv::mult(const SparseMatrix & M, const FullVector & x, FullVector & y)
|
||||
{
|
||||
mult(NoTransposed, M, x, y);
|
||||
}
|
||||
|
||||
void nv::mult(Transpose TM, const SparseMatrix & M, const FullVector & x, FullVector & y)
|
||||
{
|
||||
const uint w = M.width();
|
||||
const uint h = M.height();
|
||||
|
||||
if (TM == Transposed)
|
||||
{
|
||||
nvDebugCheck( h == x.dimension() );
|
||||
nvDebugCheck( w == y.dimension() );
|
||||
|
||||
y.fill(0.0f);
|
||||
|
||||
for (uint i = 0; i < h; i++)
|
||||
{
|
||||
M.madRow(i, x[i], y);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
nvDebugCheck( w == x.dimension() );
|
||||
nvDebugCheck( h == y.dimension() );
|
||||
|
||||
for (uint i = 0; i < h; i++)
|
||||
{
|
||||
y[i] = M.dotRow(i, x);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// y = alpha*A*x + beta*y
|
||||
void nv::sgemv(float alpha, const SparseMatrix & A, const FullVector & x, float beta, FullVector & y)
|
||||
{
|
||||
sgemv(alpha, NoTransposed, A, x, beta, y);
|
||||
}
|
||||
|
||||
void nv::sgemv(float alpha, Transpose TA, const SparseMatrix & A, const FullVector & x, float beta, FullVector & y)
|
||||
{
|
||||
const uint w = A.width();
|
||||
const uint h = A.height();
|
||||
|
||||
if (TA == Transposed)
|
||||
{
|
||||
nvDebugCheck( h == x.dimension() );
|
||||
nvDebugCheck( w == y.dimension() );
|
||||
|
||||
for (uint i = 0; i < h; i++)
|
||||
{
|
||||
A.madRow(i, alpha * x[i], y);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
nvDebugCheck( w == x.dimension() );
|
||||
nvDebugCheck( h == y.dimension() );
|
||||
|
||||
for (uint i = 0; i < h; i++)
|
||||
{
|
||||
y[i] = alpha * A.dotRow(i, x) + beta * y[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// dot y-row of A by x-column of B
|
||||
static float dotRowColumn(int y, const SparseMatrix & A, int x, const SparseMatrix & B)
|
||||
{
|
||||
const Array<SparseMatrix::Coefficient> & row = A.getRow(y);
|
||||
|
||||
const uint count = row.count();
|
||||
|
||||
/*float sum = 0.0f;
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
const SparseMatrix::Coefficient & c = row[i];
|
||||
sum += c.v * B.getCoefficient(x, c.x);
|
||||
}
|
||||
return sum;*/
|
||||
|
||||
KahanSum kahan;
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
const SparseMatrix::Coefficient & c = row[i];
|
||||
kahan.add(c.v * B.getCoefficient(x, c.x));
|
||||
}
|
||||
|
||||
return kahan.sum();
|
||||
}
|
||||
|
||||
// dot y-row of A by x-row of B
|
||||
static float dotRowRow(int y, const SparseMatrix & A, int x, const SparseMatrix & B)
|
||||
{
|
||||
const Array<SparseMatrix::Coefficient> & row = A.getRow(y);
|
||||
|
||||
const uint count = row.count();
|
||||
|
||||
/*float sum = 0.0f;
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
const SparseMatrix::Coefficient & c = row[i];
|
||||
sum += c.v * B.getCoefficient(c.x, x);
|
||||
}
|
||||
//return sum;*/
|
||||
|
||||
KahanSum kahan;
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
const SparseMatrix::Coefficient & c = row[i];
|
||||
kahan.add(c.v * B.getCoefficient(c.x, x));
|
||||
}
|
||||
|
||||
return kahan.sum();
|
||||
}
|
||||
|
||||
// dot y-column of A by x-column of B
|
||||
static float dotColumnColumn(int y, const SparseMatrix & A, int x, const SparseMatrix & B)
|
||||
{
|
||||
nvDebugCheck(A.height() == B.height());
|
||||
|
||||
const uint h = A.height();
|
||||
|
||||
/*float sum = 0.0f;
|
||||
for (uint i = 0; i < h; i++)
|
||||
{
|
||||
sum += A.getCoefficient(y, i) * B.getCoefficient(x, i);
|
||||
}
|
||||
//return sum;*/
|
||||
|
||||
KahanSum kahan;
|
||||
for (uint i = 0; i < h; i++)
|
||||
{
|
||||
kahan.add(A.getCoefficient(y, i) * B.getCoefficient(x, i));
|
||||
}
|
||||
|
||||
return kahan.sum();
|
||||
}
|
||||
|
||||
|
||||
void nv::transpose(const SparseMatrix & A, SparseMatrix & B)
|
||||
{
|
||||
nvDebugCheck(A.width() == B.height());
|
||||
nvDebugCheck(B.width() == A.height());
|
||||
|
||||
const uint w = A.width();
|
||||
for (uint x = 0; x < w; x++)
|
||||
{
|
||||
B.clearRow(x);
|
||||
}
|
||||
|
||||
const uint h = A.height();
|
||||
for (uint y = 0; y < h; y++)
|
||||
{
|
||||
const Array<SparseMatrix::Coefficient> & row = A.getRow(y);
|
||||
|
||||
const uint count = row.count();
|
||||
for (uint i = 0; i < count; i++)
|
||||
{
|
||||
const SparseMatrix::Coefficient & c = row[i];
|
||||
nvDebugCheck(c.x < w);
|
||||
|
||||
B.setCoefficient(y, c.x, c.v);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// C = A * B
|
||||
void nv::mult(const SparseMatrix & A, const SparseMatrix & B, SparseMatrix & C)
|
||||
{
|
||||
mult(NoTransposed, A, NoTransposed, B, C);
|
||||
}
|
||||
|
||||
void nv::mult(Transpose TA, const SparseMatrix & A, Transpose TB, const SparseMatrix & B, SparseMatrix & C)
|
||||
{
|
||||
sgemm(1.0f, TA, A, TB, B, 0.0f, C);
|
||||
}
|
||||
|
||||
// C = alpha*A*B + beta*C
|
||||
void nv::sgemm(float alpha, const SparseMatrix & A, const SparseMatrix & B, float beta, SparseMatrix & C)
|
||||
{
|
||||
sgemm(alpha, NoTransposed, A, NoTransposed, B, beta, C);
|
||||
}
|
||||
|
||||
void nv::sgemm(float alpha, Transpose TA, const SparseMatrix & A, Transpose TB, const SparseMatrix & B, float beta, SparseMatrix & C)
|
||||
{
|
||||
const uint w = C.width();
|
||||
const uint h = C.height();
|
||||
|
||||
uint aw = (TA == NoTransposed) ? A.width() : A.height();
|
||||
uint ah = (TA == NoTransposed) ? A.height() : A.width();
|
||||
uint bw = (TB == NoTransposed) ? B.width() : B.height();
|
||||
uint bh = (TB == NoTransposed) ? B.height() : B.width();
|
||||
|
||||
nvDebugCheck(aw == bh);
|
||||
nvDebugCheck(bw == ah);
|
||||
nvDebugCheck(w == bw);
|
||||
nvDebugCheck(h == ah);
|
||||
|
||||
|
||||
for (uint y = 0; y < h; y++)
|
||||
{
|
||||
for (uint x = 0; x < w; x++)
|
||||
{
|
||||
float c = beta * C.getCoefficient(x, y);
|
||||
|
||||
if (TA == NoTransposed && TB == NoTransposed)
|
||||
{
|
||||
// dot y-row of A by x-column of B.
|
||||
c += alpha * dotRowColumn(y, A, x, B);
|
||||
}
|
||||
else if (TA == Transposed && TB == Transposed)
|
||||
{
|
||||
// dot y-column of A by x-row of B.
|
||||
c += alpha * dotRowColumn(x, B, y, A);
|
||||
}
|
||||
else if (TA == Transposed && TB == NoTransposed)
|
||||
{
|
||||
// dot y-column of A by x-column of B.
|
||||
c += alpha * dotColumnColumn(y, A, x, B);
|
||||
}
|
||||
else if (TA == NoTransposed && TB == Transposed)
|
||||
{
|
||||
// dot y-row of A by x-row of B.
|
||||
c += alpha * dotRowRow(y, A, x, B);
|
||||
}
|
||||
|
||||
C.setCoefficient(x, y, c);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// C = At * A
|
||||
void nv::sqm(const SparseMatrix & A, SparseMatrix & C)
|
||||
{
|
||||
// This is quite expensive...
|
||||
mult(Transposed, A, NoTransposed, A, C);
|
||||
}
|
||||
@ -1,200 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_SPARSE_H
|
||||
#define NV_MATH_SPARSE_H
|
||||
|
||||
#include <nvcore/Containers.h>
|
||||
#include <nvmath/nvmath.h>
|
||||
|
||||
// Full and sparse vector and matrix classes. BLAS subset.
|
||||
|
||||
namespace nv
|
||||
{
|
||||
class FullVector;
|
||||
class FullMatrix;
|
||||
class SparseMatrix;
|
||||
|
||||
|
||||
/// Fixed size vector class.
|
||||
class FullVector
|
||||
{
|
||||
public:
|
||||
|
||||
FullVector(uint dim);
|
||||
FullVector(const FullVector & v);
|
||||
|
||||
const FullVector & operator=(const FullVector & v);
|
||||
|
||||
uint dimension() const { return m_array.count(); }
|
||||
|
||||
const float & operator[]( uint index ) const { return m_array[index]; }
|
||||
float & operator[] ( uint index ) { return m_array[index]; }
|
||||
|
||||
void fill(float f);
|
||||
|
||||
void operator+= (const FullVector & v);
|
||||
void operator-= (const FullVector & v);
|
||||
void operator*= (const FullVector & v);
|
||||
|
||||
void operator+= (float f);
|
||||
void operator-= (float f);
|
||||
void operator*= (float f);
|
||||
|
||||
|
||||
private:
|
||||
|
||||
Array<float> m_array;
|
||||
|
||||
};
|
||||
|
||||
// Pseudo-BLAS interface.
|
||||
NVMATH_API void saxpy(float a, const FullVector & x, FullVector & y); // y = a * x + y
|
||||
NVMATH_API void copy(const FullVector & x, FullVector & y);
|
||||
NVMATH_API void scal(float a, FullVector & x);
|
||||
NVMATH_API float dot(const FullVector & x, const FullVector & y);
|
||||
|
||||
|
||||
enum Transpose
|
||||
{
|
||||
NoTransposed = 0,
|
||||
Transposed = 1
|
||||
};
|
||||
|
||||
/// Full matrix class.
|
||||
class FullMatrix
|
||||
{
|
||||
public:
|
||||
|
||||
FullMatrix(uint d);
|
||||
FullMatrix(uint w, uint h);
|
||||
FullMatrix(const FullMatrix & m);
|
||||
|
||||
const FullMatrix & operator=(const FullMatrix & m);
|
||||
|
||||
uint width() const { return m_width; }
|
||||
uint height() const { return m_height; }
|
||||
bool isSquare() const { return m_width == m_height; }
|
||||
|
||||
float getCoefficient(uint x, uint y) const;
|
||||
|
||||
void setCoefficient(uint x, uint y, float f);
|
||||
void addCoefficient(uint x, uint y, float f);
|
||||
void mulCoefficient(uint x, uint y, float f);
|
||||
|
||||
float dotRow(uint y, const FullVector & v) const;
|
||||
void madRow(uint y, float alpha, FullVector & v) const;
|
||||
|
||||
protected:
|
||||
|
||||
bool isValid() const {
|
||||
return m_array.size() == (m_width * m_height);
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
const uint m_width;
|
||||
const uint m_height;
|
||||
Array<float> m_array;
|
||||
|
||||
};
|
||||
|
||||
NVMATH_API void mult(const FullMatrix & M, const FullVector & x, FullVector & y);
|
||||
NVMATH_API void mult(Transpose TM, const FullMatrix & M, const FullVector & x, FullVector & y);
|
||||
|
||||
// y = alpha*A*x + beta*y
|
||||
NVMATH_API void sgemv(float alpha, const FullMatrix & A, const FullVector & x, float beta, FullVector & y);
|
||||
NVMATH_API void sgemv(float alpha, Transpose TA, const FullMatrix & A, const FullVector & x, float beta, FullVector & y);
|
||||
|
||||
NVMATH_API void mult(const FullMatrix & A, const FullMatrix & B, FullMatrix & C);
|
||||
NVMATH_API void mult(Transpose TA, const FullMatrix & A, Transpose TB, const FullMatrix & B, FullMatrix & C);
|
||||
|
||||
// C = alpha*A*B + beta*C
|
||||
NVMATH_API void sgemm(float alpha, const FullMatrix & A, const FullMatrix & B, float beta, FullMatrix & C);
|
||||
NVMATH_API void sgemm(float alpha, Transpose TA, const FullMatrix & A, Transpose TB, const FullMatrix & B, float beta, FullMatrix & C);
|
||||
|
||||
|
||||
/**
|
||||
* Sparse matrix class. The matrix is assumed to be sparse and to have
|
||||
* very few non-zero elements, for this reason it's stored in indexed
|
||||
* format. To multiply column vectors efficiently, the matrix stores
|
||||
* the elements in indexed-column order, there is a list of indexed
|
||||
* elements for each row of the matrix. As with the FullVector the
|
||||
* dimension of the matrix is constant.
|
||||
**/
|
||||
class SparseMatrix
|
||||
{
|
||||
friend class FullMatrix;
|
||||
public:
|
||||
|
||||
// An element of the sparse array.
|
||||
struct Coefficient {
|
||||
uint x; // column
|
||||
float v; // value
|
||||
};
|
||||
|
||||
|
||||
public:
|
||||
|
||||
SparseMatrix(uint d);
|
||||
SparseMatrix(uint w, uint h);
|
||||
SparseMatrix(const SparseMatrix & m);
|
||||
|
||||
const SparseMatrix & operator=(const SparseMatrix & m);
|
||||
|
||||
|
||||
uint width() const { return m_width; }
|
||||
uint height() const { return m_array.count(); }
|
||||
bool isSquare() const { return width() == height(); }
|
||||
|
||||
float getCoefficient(uint x, uint y) const; // x is column, y is row
|
||||
|
||||
void setCoefficient(uint x, uint y, float f);
|
||||
void addCoefficient(uint x, uint y, float f);
|
||||
void mulCoefficient(uint x, uint y, float f);
|
||||
|
||||
float sumRow(uint y) const;
|
||||
float dotRow(uint y, const FullVector & v) const;
|
||||
void madRow(uint y, float alpha, FullVector & v) const;
|
||||
|
||||
void clearRow(uint y);
|
||||
void scaleRow(uint y, float f);
|
||||
void normalizeRow(uint y);
|
||||
|
||||
void clearColumn(uint x);
|
||||
void scaleColumn(uint x, float f);
|
||||
|
||||
const Array<Coefficient> & getRow(uint y) const;
|
||||
|
||||
private:
|
||||
|
||||
/// Number of columns.
|
||||
const uint m_width;
|
||||
|
||||
/// Array of matrix elements.
|
||||
Array< Array<Coefficient> > m_array;
|
||||
|
||||
};
|
||||
|
||||
NVMATH_API void transpose(const SparseMatrix & A, SparseMatrix & B);
|
||||
|
||||
NVMATH_API void mult(const SparseMatrix & M, const FullVector & x, FullVector & y);
|
||||
NVMATH_API void mult(Transpose TM, const SparseMatrix & M, const FullVector & x, FullVector & y);
|
||||
|
||||
// y = alpha*A*x + beta*y
|
||||
NVMATH_API void sgemv(float alpha, const SparseMatrix & A, const FullVector & x, float beta, FullVector & y);
|
||||
NVMATH_API void sgemv(float alpha, Transpose TA, const SparseMatrix & A, const FullVector & x, float beta, FullVector & y);
|
||||
|
||||
NVMATH_API void mult(const SparseMatrix & A, const SparseMatrix & B, SparseMatrix & C);
|
||||
NVMATH_API void mult(Transpose TA, const SparseMatrix & A, Transpose TB, const SparseMatrix & B, SparseMatrix & C);
|
||||
|
||||
// C = alpha*A*B + beta*C
|
||||
NVMATH_API void sgemm(float alpha, const SparseMatrix & A, const SparseMatrix & B, float beta, SparseMatrix & C);
|
||||
NVMATH_API void sgemm(float alpha, Transpose TA, const SparseMatrix & A, Transpose TB, const SparseMatrix & B, float beta, SparseMatrix & C);
|
||||
|
||||
// C = At * A
|
||||
NVMATH_API void sqm(const SparseMatrix & A, SparseMatrix & C);
|
||||
|
||||
} // nv namespace
|
||||
|
||||
|
||||
#endif // NV_MATH_SPARSE_H
|
||||
@ -1,243 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#include <nvmath/SphericalHarmonic.h>
|
||||
|
||||
using namespace nv;
|
||||
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
// Basic integer factorial.
|
||||
inline static int factorial( int v )
|
||||
{
|
||||
const static int fac_table[] = { 1, 1, 2, 6, 24, 120, 720, 5040, 40320, 362880, 3628800, 39916800 };
|
||||
|
||||
if(v <= 11){
|
||||
return fac_table[v];
|
||||
}
|
||||
|
||||
int result = v;
|
||||
while (--v > 0) {
|
||||
result *= v;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
// Double factorial.
|
||||
// Defined as: n!! = n*(n - 2)*(n - 4)..., n!!(0,-1) = 1.
|
||||
inline static int doubleFactorial( int x )
|
||||
{
|
||||
if (x == 0 || x == -1) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
int result = x;
|
||||
while ((x -= 2) > 0) {
|
||||
result *= x;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// Normalization constant for spherical harmonic.
|
||||
/// @param l is the band.
|
||||
/// @param m is the argument, in the range [0, m]
|
||||
inline static float K( int l, int m )
|
||||
{
|
||||
nvDebugCheck( m >= 0 );
|
||||
return sqrtf(((2 * l + 1) * factorial(l - m)) / (4 * PI * factorial(l + m)));
|
||||
}
|
||||
|
||||
/// Normalization constant for hemispherical harmonic.
|
||||
inline static float HK( int l, int m )
|
||||
{
|
||||
nvDebugCheck( m >= 0 );
|
||||
return sqrtf(((2 * l + 1) * factorial(l - m)) / (2 * PI * factorial(l + m)));
|
||||
}
|
||||
|
||||
/// Evaluate Legendre polynomial. */
|
||||
static float legendre( int l, int m, float x )
|
||||
{
|
||||
// piDebugCheck( m >= 0 );
|
||||
// piDebugCheck( m <= l );
|
||||
// piDebugCheck( fabs(x) <= 1 );
|
||||
|
||||
// Rule 2 needs no previous results
|
||||
if (l == m) {
|
||||
return powf(-1.0f, float(m)) * doubleFactorial(2 * m - 1) * powf(1 - x*x, 0.5f * m);
|
||||
}
|
||||
|
||||
// Rule 3 requires the result for the same argument of the previous band
|
||||
if (l == m + 1) {
|
||||
return x * (2 * m + 1) * legendrePolynomial(m, m, x);
|
||||
}
|
||||
|
||||
// Main reccurence used by rule 1 that uses result of the same argument from
|
||||
// the previous two bands
|
||||
return (x * (2 * l - 1) * legendrePolynomial(l - 1, m, x) - (l + m - 1) * legendrePolynomial(l - 2, m, x)) / (l - m);
|
||||
}
|
||||
|
||||
|
||||
template <int l, int m> float legendre(float x);
|
||||
|
||||
template <> float legendre<0, 0>(float ) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
template <> float legendre<1, 0>(float x) {
|
||||
return x;
|
||||
}
|
||||
template <> float legendre<1, 1>(float x) {
|
||||
return -sqrtf(1 - x * x);
|
||||
}
|
||||
|
||||
template <> float legendre<2, 0>(float x) {
|
||||
return -0.5f + (3 * x * x) / 2;
|
||||
}
|
||||
template <> float legendre<2, 1>(float x) {
|
||||
return -3 * x * sqrtf(1 - x * x);
|
||||
}
|
||||
template <> float legendre<2, 2>(float x) {
|
||||
return -3 * (-1 + x * x);
|
||||
}
|
||||
|
||||
template <> float legendre<3, 0>(float x) {
|
||||
return -(3 * x) / 2 + (5 * x * x * x) / 2;
|
||||
}
|
||||
template <> float legendre<3, 1>(float x) {
|
||||
return -3 * sqrtf(1 - x * x) / 2 * (-1 + 5 * x * x);
|
||||
}
|
||||
template <> float legendre<3, 2>(float x) {
|
||||
return -15 * (-x + x * x * x);
|
||||
}
|
||||
template <> float legendre<3, 3>(float x) {
|
||||
return -15 * powf(1 - x * x, 1.5f);
|
||||
}
|
||||
|
||||
template <> float legendre<4, 0>(float x) {
|
||||
return 0.125f * (3.0f - 30.0f * x * x + 35.0f * x * x * x * x);
|
||||
}
|
||||
template <> float legendre<4, 1>(float x) {
|
||||
return -2.5f * x * sqrtf(1.0f - x * x) * (7.0f * x * x - 3.0f);
|
||||
}
|
||||
template <> float legendre<4, 2>(float x) {
|
||||
return -7.5f * (1.0f - 8.0f * x * x + 7.0f * x * x * x * x);
|
||||
}
|
||||
template <> float legendre<4, 3>(float x) {
|
||||
return -105.0f * x * powf(1 - x * x, 1.5f);
|
||||
}
|
||||
template <> float legendre<4, 4>(float x) {
|
||||
return 105.0f * (x * x - 1.0f) * (x * x - 1.0f);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
|
||||
float nv::legendrePolynomial(int l, int m, float x)
|
||||
{
|
||||
switch(l)
|
||||
{
|
||||
case 0:
|
||||
return legendre<0, 0>(x);
|
||||
case 1:
|
||||
if(m == 0) return legendre<1, 0>(x);
|
||||
return legendre<1, 1>(x);
|
||||
case 2:
|
||||
if(m == 0) return legendre<2, 0>(x);
|
||||
else if(m == 1) return legendre<2, 1>(x);
|
||||
return legendre<2, 2>(x);
|
||||
case 3:
|
||||
if(m == 0) return legendre<3, 0>(x);
|
||||
else if(m == 1) return legendre<3, 1>(x);
|
||||
else if(m == 2) return legendre<3, 2>(x);
|
||||
return legendre<3, 3>(x);
|
||||
case 4:
|
||||
if(m == 0) return legendre<4, 0>(x);
|
||||
else if(m == 1) return legendre<4, 1>(x);
|
||||
else if(m == 2) return legendre<4, 2>(x);
|
||||
else if(m == 3) return legendre<4, 3>(x);
|
||||
else return legendre<4, 4>(x);
|
||||
}
|
||||
|
||||
// Fallback to the expensive version.
|
||||
return legendre(l, m, x);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Evaluate the spherical harmonic function for the given angles.
|
||||
* @param l is the band.
|
||||
* @param m is the argument, in the range [-l,l]
|
||||
* @param theta is the altitude, in the range [0, PI]
|
||||
* @param phi is the azimuth, in the range [0, 2*PI]
|
||||
*/
|
||||
float nv::y( int l, int m, float theta, float phi )
|
||||
{
|
||||
if( m == 0 ) {
|
||||
// K(l, 0) = sqrt((2*l+1)/(4*PI))
|
||||
return sqrtf((2 * l + 1) / (4 * PI)) * legendrePolynomial(l, 0, cosf(theta));
|
||||
}
|
||||
else if( m > 0 ) {
|
||||
return sqrtf(2.0f) * K(l, m) * cosf(m * phi) * legendrePolynomial(l, m, cosf(theta));
|
||||
}
|
||||
else {
|
||||
return sqrtf(2.0f) * K(l, -m) * sinf(-m * phi) * legendrePolynomial(l, -m, cosf(theta));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Real spherical harmonic function of an unit vector. Uses the following
|
||||
* equalities to call the angular function:
|
||||
* x = sin(theta)*cos(phi)
|
||||
* y = sin(theta)*sin(phi)
|
||||
* z = cos(theta)
|
||||
*/
|
||||
float nv::y( int l, int m, Vector3::Arg v )
|
||||
{
|
||||
float theta = acosf(v.z());
|
||||
float phi = atan2f(v.y(), v.x());
|
||||
return y( l, m, theta, phi );
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Evaluate the hemispherical harmonic function for the given angles.
|
||||
* @param l is the band.
|
||||
* @param m is the argument, in the range [-l,l]
|
||||
* @param theta is the altitude, in the range [0, PI/2]
|
||||
* @param phi is the azimuth, in the range [0, 2*PI]
|
||||
*/
|
||||
float nv::hy( int l, int m, float theta, float phi )
|
||||
{
|
||||
if( m == 0 ) {
|
||||
// HK(l, 0) = sqrt((2*l+1)/(2*PI))
|
||||
return sqrtf((2 * l + 1) / (2 * PI)) * legendrePolynomial(l, 0, 2*cosf(theta)-1);
|
||||
}
|
||||
else if( m > 0 ) {
|
||||
return sqrtf(2.0f) * HK(l, m) * cosf(m * phi) * legendrePolynomial(l, m, 2*cosf(theta)-1);
|
||||
}
|
||||
else {
|
||||
return sqrtf(2.0f) * HK(l, -m) * sinf(-m * phi) * legendrePolynomial(l, -m, 2*cosf(theta)-1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Real hemispherical harmonic function of an unit vector. Uses the following
|
||||
* equalities to call the angular function:
|
||||
* x = sin(theta)*cos(phi)
|
||||
* y = sin(theta)*sin(phi)
|
||||
* z = cos(theta)
|
||||
*/
|
||||
float nv::hy( int l, int m, Vector3::Arg v )
|
||||
{
|
||||
float theta = acosf(v.z());
|
||||
float phi = atan2f(v.y(), v.x());
|
||||
return y( l, m, theta, phi );
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -1,420 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_SPHERICALHARMONIC_H
|
||||
#define NV_MATH_SPHERICALHARMONIC_H
|
||||
|
||||
#include <string.h> // memcpy
|
||||
#include <nvmath/Vector.h>
|
||||
|
||||
namespace nv
|
||||
{
|
||||
|
||||
NVMATH_API float legendrePolynomial( int l, int m, float x ) NV_CONST;
|
||||
NVMATH_API float y( int l, int m, float theta, float phi ) NV_CONST;
|
||||
NVMATH_API float y( int l, int m, Vector3::Arg v ) NV_CONST;
|
||||
NVMATH_API float hy( int l, int m, float theta, float phi ) NV_CONST;
|
||||
NVMATH_API float hy( int l, int m, Vector3::Arg v ) NV_CONST;
|
||||
|
||||
class Sh;
|
||||
float dot(const Sh & a, const Sh & b) NV_CONST;
|
||||
|
||||
|
||||
/// Spherical harmonic class.
|
||||
class Sh
|
||||
{
|
||||
friend class Sh2;
|
||||
friend class ShMatrix;
|
||||
public:
|
||||
|
||||
/// Construct a spherical harmonic of the given order.
|
||||
Sh(int o) : m_order(o)
|
||||
{
|
||||
m_elemArray = new float[basisNum()];
|
||||
}
|
||||
|
||||
/// Copy constructor.
|
||||
Sh(const Sh & sh) : m_order(sh.order())
|
||||
{
|
||||
m_elemArray = new float[basisNum()];
|
||||
memcpy(m_elemArray, sh.m_elemArray, sizeof(float) * basisNum());
|
||||
}
|
||||
|
||||
/// Destructor.
|
||||
~Sh()
|
||||
{
|
||||
delete [] m_elemArray;
|
||||
m_elemArray = NULL;
|
||||
}
|
||||
|
||||
/// Get number of bands.
|
||||
static int bandNum(int order) {
|
||||
return order + 1;
|
||||
}
|
||||
|
||||
/// Get number of sh basis.
|
||||
static int basisNum(int order) {
|
||||
return (order + 1) * (order + 1);
|
||||
}
|
||||
|
||||
/// Get the index for the given coefficients.
|
||||
static int index( int l, int m ) {
|
||||
return l * l + l + m;
|
||||
}
|
||||
|
||||
/// Get sh order.
|
||||
int order() const
|
||||
{
|
||||
return m_order;
|
||||
}
|
||||
|
||||
/// Get sh order.
|
||||
int bandNum() const
|
||||
{
|
||||
return bandNum(m_order);
|
||||
}
|
||||
|
||||
/// Get sh order.
|
||||
int basisNum() const
|
||||
{
|
||||
return basisNum(m_order);
|
||||
}
|
||||
|
||||
/// Get sh coefficient indexed by l,m.
|
||||
float elem( int l, int m ) const
|
||||
{
|
||||
return m_elemArray[index(l, m)];
|
||||
}
|
||||
|
||||
/// Get sh coefficient indexed by l,m.
|
||||
float & elem( int l, int m )
|
||||
{
|
||||
return m_elemArray[index(l, m)];
|
||||
}
|
||||
|
||||
|
||||
/// Get sh coefficient indexed by i.
|
||||
float elemAt( int i ) const {
|
||||
return m_elemArray[i];
|
||||
}
|
||||
|
||||
/// Get sh coefficient indexed by i.
|
||||
float & elemAt( int i )
|
||||
{
|
||||
return m_elemArray[i];
|
||||
}
|
||||
|
||||
|
||||
/// Reset the sh coefficients.
|
||||
void reset()
|
||||
{
|
||||
for( int i = 0; i < basisNum(); i++ ) {
|
||||
m_elemArray[i] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/// Copy spherical harmonic.
|
||||
void operator= ( const Sh & sh )
|
||||
{
|
||||
nvDebugCheck(order() <= sh.order());
|
||||
|
||||
for(int i = 0; i < basisNum(); i++) {
|
||||
m_elemArray[i] = sh.m_elemArray[i];
|
||||
}
|
||||
}
|
||||
|
||||
/// Add spherical harmonics.
|
||||
void operator+= ( const Sh & sh )
|
||||
{
|
||||
nvDebugCheck(order() == sh.order());
|
||||
|
||||
for(int i = 0; i < basisNum(); i++) {
|
||||
m_elemArray[i] += sh.m_elemArray[i];
|
||||
}
|
||||
}
|
||||
|
||||
/// Substract spherical harmonics.
|
||||
void operator-= ( const Sh & sh )
|
||||
{
|
||||
nvDebugCheck(order() == sh.order());
|
||||
|
||||
for(int i = 0; i < basisNum(); i++) {
|
||||
m_elemArray[i] -= sh.m_elemArray[i];
|
||||
}
|
||||
}
|
||||
|
||||
// Not exactly convolution, nor product.
|
||||
void operator*= ( const Sh & sh )
|
||||
{
|
||||
nvDebugCheck(order() == sh.order());
|
||||
|
||||
for(int i = 0; i < basisNum(); i++) {
|
||||
m_elemArray[i] *= sh.m_elemArray[i];
|
||||
}
|
||||
}
|
||||
|
||||
/// Scale spherical harmonics.
|
||||
void operator*= ( float f )
|
||||
{
|
||||
for(int i = 0; i < basisNum(); i++) {
|
||||
m_elemArray[i] *= f;
|
||||
}
|
||||
}
|
||||
|
||||
/// Add scaled spherical harmonics.
|
||||
void addScaled( const Sh & sh, float f )
|
||||
{
|
||||
nvDebugCheck(order() == sh.order());
|
||||
|
||||
for(int i = 0; i < basisNum(); i++) {
|
||||
m_elemArray[i] += sh.m_elemArray[i] * f;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/*/// Add a weighted sample to the sh coefficients.
|
||||
void AddSample( const Vec3 & dir, const Color3f & color, float w=1.0f ) {
|
||||
for(int l = 0; l <= order; l++) {
|
||||
for(int m = -l; m <= l; m++) {
|
||||
Color3f & elem = GetElem(l, m);
|
||||
elem.Mad( elem, color, w * y(l, m, dir) );
|
||||
}
|
||||
}
|
||||
}*/
|
||||
|
||||
/// Evaluate
|
||||
void eval(Vector3::Arg dir)
|
||||
{
|
||||
for(int l = 0; l <= m_order; l++) {
|
||||
for(int m = -l; m <= l; m++) {
|
||||
elem(l, m) = y(l, m, dir);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/// Evaluate the spherical harmonic function.
|
||||
float sample(Vector3::Arg dir) const
|
||||
{
|
||||
Sh sh(order());
|
||||
sh.eval(dir);
|
||||
|
||||
return dot(sh, *this);
|
||||
}
|
||||
|
||||
|
||||
protected:
|
||||
|
||||
const int m_order;
|
||||
float * m_elemArray;
|
||||
|
||||
};
|
||||
|
||||
|
||||
/// Compute dot product of the spherical harmonics.
|
||||
inline float dot(const Sh & a, const Sh & b)
|
||||
{
|
||||
nvDebugCheck(a.order() == b.order());
|
||||
|
||||
float sum = 0;
|
||||
for( int i = 0; i < Sh::basisNum(a.order()); i++ ) {
|
||||
sum += a.elemAt(i) * b.elemAt(i);
|
||||
}
|
||||
|
||||
return sum;
|
||||
}
|
||||
|
||||
|
||||
/// Second order spherical harmonic.
|
||||
class Sh2 : public Sh
|
||||
{
|
||||
public:
|
||||
|
||||
/// Constructor.
|
||||
Sh2() : Sh(2) {}
|
||||
|
||||
/// Copy constructor.
|
||||
Sh2(const Sh2 & sh) : Sh(sh) {}
|
||||
|
||||
/// Spherical harmonic resulting from projecting the clamped cosine transfer function to the SH basis.
|
||||
void cosineTransfer()
|
||||
{
|
||||
const float c1 = 0.282095f; // K(0, 0)
|
||||
const float c2 = 0.488603f; // K(1, 0)
|
||||
const float c3 = 1.092548f; // sqrt(15.0f / PI) / 2.0f = K(2, -2)
|
||||
const float c4 = 0.315392f; // sqrt(5.0f / PI) / 4.0f) = K(2, 0)
|
||||
const float c5 = 0.546274f; // sqrt(15.0f / PI) / 4.0f) = K(2, 2)
|
||||
|
||||
const float normalization = PI * 16.0f / 17.0f;
|
||||
|
||||
const float const1 = c1 * normalization * 1.0f;
|
||||
const float const2 = c2 * normalization * (2.0f / 3.0f);
|
||||
const float const3 = c3 * normalization * (1.0f / 4.0f);
|
||||
const float const4 = c4 * normalization * (1.0f / 4.0f);
|
||||
const float const5 = c5 * normalization * (1.0f / 4.0f);
|
||||
|
||||
m_elemArray[0] = const1;
|
||||
|
||||
m_elemArray[1] = -const2;
|
||||
m_elemArray[2] = const2;
|
||||
m_elemArray[3] = -const2;
|
||||
|
||||
m_elemArray[4] = const3;
|
||||
m_elemArray[5] = -const3;
|
||||
m_elemArray[6] = const4;
|
||||
m_elemArray[7] = -const3;
|
||||
m_elemArray[8] = const5;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
#if 0
|
||||
|
||||
/// Spherical harmonic matrix.
|
||||
class ShMatrix
|
||||
{
|
||||
public:
|
||||
|
||||
/// Create an identity matrix of the given order.
|
||||
ShMatrix(int o = 2) : order(o), identity(true)
|
||||
{
|
||||
nvCheck(order > 0);
|
||||
e = new float[Size()];
|
||||
band = new float *[GetBandNum()];
|
||||
setupBands();
|
||||
}
|
||||
|
||||
/// Destroy and free matrix elements.
|
||||
~ShMatrix()
|
||||
{
|
||||
delete e;
|
||||
delete band;
|
||||
}
|
||||
|
||||
/// Set identity matrix.
|
||||
void setIdentity()
|
||||
{
|
||||
identity = true;
|
||||
}
|
||||
|
||||
/// Return true if this is an identity matrix, false in other case.
|
||||
bool isIdentity() const {
|
||||
return identity;
|
||||
}
|
||||
|
||||
/// Get number of bands of this matrix.
|
||||
int bandNum() const
|
||||
{
|
||||
return order+1;
|
||||
}
|
||||
|
||||
/// Get total number of elements in the matrix.
|
||||
int size() const
|
||||
{
|
||||
int size = 0;
|
||||
for( int i = 0; i < bandNum(); i++ ) {
|
||||
size += SQ(i * 2 + 1);
|
||||
}
|
||||
return size;
|
||||
}
|
||||
|
||||
/// Get element at the given raw index.
|
||||
float elem(const int idx) const
|
||||
{
|
||||
return e[idx];
|
||||
}
|
||||
|
||||
/// Get element at the given with the given indices.
|
||||
float & elem( const int b, const int x, const int y )
|
||||
{
|
||||
nvDebugCheck(b >= 0);
|
||||
nvDebugCheck(b < bandNum());
|
||||
return band[b][(b + y) * (b * 2 + 1) + (b + x)];
|
||||
}
|
||||
|
||||
/// Get element at the given with the given indices.
|
||||
float elem( const int b, const int x, const int y ) const
|
||||
{
|
||||
nvDebugCheck(b >= 0);
|
||||
nvDebugCheck(b < bandNum());
|
||||
return band[b][(b + y) * (b * 2 + 1) + (b + x)];
|
||||
}
|
||||
|
||||
/** Copy matrix. */
|
||||
void Copy( const ShMatrix & m )
|
||||
{
|
||||
nvDebugCheck(order == m.order);
|
||||
memcpy(e, m.e, Size() * sizeof(float));
|
||||
}
|
||||
|
||||
/** Rotate the given coefficients. */
|
||||
void transform( const Sh & restrict source, Sh * restrict dest ) const {
|
||||
piCheck( &source != dest ); // Make sure there's no aliasing.
|
||||
piCheck( dest->order <= order );
|
||||
piCheck( order <= source.order );
|
||||
|
||||
if( identity ) {
|
||||
*dest = source;
|
||||
return;
|
||||
}
|
||||
|
||||
// Loop through each band.
|
||||
for( int l = 0; l <= dest->order; l++ ) {
|
||||
|
||||
for( int mo = -l; mo <= l; mo++ ) {
|
||||
|
||||
Color3f rgb = Color3f::Black;
|
||||
|
||||
for( int mi = -l; mi <= l; mi++ ) {
|
||||
rgb.Mad( rgb, source.elem(l, mi), elem(l, mo, mi) );
|
||||
}
|
||||
|
||||
dest->elem(l, mo) = rgb;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
MATHLIB_API void multiply( const ShMatrix &A, const ShMatrix &B );
|
||||
MATHLIB_API void rotation( const Matrix & m );
|
||||
MATHLIB_API void rotation( int axis, float angles );
|
||||
MATHLIB_API void print();
|
||||
|
||||
|
||||
private:
|
||||
|
||||
// @@ These could be static indices precomputed only once.
|
||||
/// Setup the band pointers.
|
||||
void setupBands()
|
||||
{
|
||||
int size = 0;
|
||||
for( int i = 0; i < bandNum(); i++ ) {
|
||||
band[i] = &e[size];
|
||||
size += SQ(i * 2 + 1);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
|
||||
// Matrix order.
|
||||
const int m_order;
|
||||
|
||||
// Identity flag for quick transform.
|
||||
bool m_identity;
|
||||
|
||||
// Array of elements.
|
||||
float * m_e;
|
||||
|
||||
// Band pointers.
|
||||
float ** m_band;
|
||||
|
||||
};
|
||||
|
||||
#endif // 0
|
||||
|
||||
|
||||
|
||||
} // nv namespace
|
||||
|
||||
#endif // NV_MATH_SPHERICALHARMONIC_H
|
||||
@ -1,226 +0,0 @@
|
||||
/********************************************************/
|
||||
/* AABB-triangle overlap test code */
|
||||
/* by Tomas Akenine-M<>ller */
|
||||
/* Function: int triBoxOverlap(float boxcenter[3], */
|
||||
/* float boxhalfsize[3],float triverts[3][3]); */
|
||||
/* History: */
|
||||
/* 2001-03-05: released the code in its first version */
|
||||
/* 2001-06-18: changed the order of the tests, faster */
|
||||
/* */
|
||||
/* Acknowledgement: Many thanks to Pierre Terdiman for */
|
||||
/* suggestions and discussions on how to optimize code. */
|
||||
/* Thanks to David Hunt for finding a ">="-bug! */
|
||||
/********************************************************/
|
||||
|
||||
#include <nvmath/Vector.h>
|
||||
#include <nvmath/Triangle.h>
|
||||
|
||||
using namespace nv;
|
||||
|
||||
#define X 0
|
||||
#define Y 1
|
||||
#define Z 2
|
||||
|
||||
#define FINDMINMAX(x0,x1,x2,min,max) \
|
||||
min = max = x0; \
|
||||
if(x1<min) min=x1;\
|
||||
if(x1>max) max=x1;\
|
||||
if(x2<min) min=x2;\
|
||||
if(x2>max) max=x2;
|
||||
|
||||
|
||||
static bool planeBoxOverlap(Vector3::Arg normal, Vector3::Arg vert, Vector3::Arg maxbox) // -NJMP-
|
||||
{
|
||||
Vector3 vmin, vmax;
|
||||
|
||||
float signs[3] = {1, 1, 1};
|
||||
if (normal.x() <= 0.0f) signs[0] = -1;
|
||||
if (normal.y() <= 0.0f) signs[1] = -1;
|
||||
if (normal.z() <= 0.0f) signs[2] = -1;
|
||||
|
||||
Vector3 sign(signs[0], signs[1], signs[2]);
|
||||
vmin = -scale(sign, maxbox) - vert;
|
||||
vmax = scale(sign, maxbox) - vert;
|
||||
|
||||
if (dot(normal, vmin) > 0.0f) return false;
|
||||
if (dot(normal, vmax) >= 0.0f) return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
/*======================== X-tests ========================*/
|
||||
#define AXISTEST_X01(a, b, fa, fb) \
|
||||
p0 = a*v0.y() - b*v0.z(); \
|
||||
p2 = a*v2.y() - b*v2.z(); \
|
||||
if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
|
||||
rad = fa * boxhalfsize.y() + fb * boxhalfsize.z(); \
|
||||
if(min>rad || max<-rad) return false;
|
||||
|
||||
#define AXISTEST_X2(a, b, fa, fb) \
|
||||
p0 = a*v0.y() - b*v0.z(); \
|
||||
p1 = a*v1.y() - b*v1.z(); \
|
||||
if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
|
||||
rad = fa * boxhalfsize.y() + fb * boxhalfsize.z(); \
|
||||
if(min>rad || max<-rad) return false;
|
||||
|
||||
/*======================== Y-tests ========================*/
|
||||
#define AXISTEST_Y02(a, b, fa, fb) \
|
||||
p0 = -a*v0.x() + b*v0.z(); \
|
||||
p2 = -a*v2.x() + b*v2.z(); \
|
||||
if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
|
||||
rad = fa * boxhalfsize.x() + fb * boxhalfsize.z(); \
|
||||
if(min>rad || max<-rad) return false;
|
||||
|
||||
#define AXISTEST_Y1(a, b, fa, fb) \
|
||||
p0 = -a*v0.x() + b*v0.z(); \
|
||||
p1 = -a*v1.x() + b*v1.z(); \
|
||||
if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
|
||||
rad = fa * boxhalfsize.x() + fb * boxhalfsize.z(); \
|
||||
if(min>rad || max<-rad) return false;
|
||||
|
||||
/*======================== Z-tests ========================*/
|
||||
|
||||
#define AXISTEST_Z12(a, b, fa, fb) \
|
||||
p1 = a*v1.x() - b*v1.y(); \
|
||||
p2 = a*v2.x() - b*v2.y(); \
|
||||
if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;} \
|
||||
rad = fa * boxhalfsize.x() + fb * boxhalfsize.y(); \
|
||||
if(min>rad || max<-rad) return false;
|
||||
|
||||
#define AXISTEST_Z0(a, b, fa, fb) \
|
||||
p0 = a*v0.x() - b*v0.y(); \
|
||||
p1 = a*v1.x() - b*v1.y(); \
|
||||
if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
|
||||
rad = fa * boxhalfsize.x() + fb * boxhalfsize.y(); \
|
||||
if(min>rad || max<-rad) return false;
|
||||
|
||||
|
||||
bool nv::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:
|
||||
// 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
|
||||
// we do not even need to test these)
|
||||
// 2) normal of the triangle
|
||||
// 3) crossproduct(edge from tri, {x,y,z}-directin)
|
||||
// this gives 3x3=9 more tests
|
||||
Vector3 v0, v1, v2;
|
||||
float min, max, p0, p1, p2, rad, fex, fey, fez;
|
||||
Vector3 normal, e0, e1, e2;
|
||||
|
||||
// This is the fastest branch on Sun.
|
||||
// move everything so that the boxcenter is in (0,0,0)
|
||||
v0 = tri.v[0] - boxcenter;
|
||||
v1 = tri.v[1] - boxcenter;
|
||||
v2 = tri.v[2] - boxcenter;
|
||||
|
||||
// Compute triangle edges.
|
||||
e0 = v1 - v0; // tri edge 0
|
||||
e1 = v2 - v1; // tri edge 1
|
||||
e2 = v0 - v2; // tri edge 2
|
||||
|
||||
// Bullet 3:
|
||||
// test the 9 tests first (this was faster)
|
||||
fex = fabsf(e0.x());
|
||||
fey = fabsf(e0.y());
|
||||
fez = fabsf(e0.z());
|
||||
AXISTEST_X01(e0.z(), e0.y(), fez, fey);
|
||||
AXISTEST_Y02(e0.z(), e0.x(), fez, fex);
|
||||
AXISTEST_Z12(e0.y(), e0.x(), fey, fex);
|
||||
|
||||
fex = fabsf(e1.x());
|
||||
fey = fabsf(e1.y());
|
||||
fez = fabsf(e1.z());
|
||||
AXISTEST_X01(e1.z(), e1.y(), fez, fey);
|
||||
AXISTEST_Y02(e1.z(), e1.x(), fez, fex);
|
||||
AXISTEST_Z0(e1.y(), e1.x(), fey, fex);
|
||||
|
||||
fex = fabsf(e2.x());
|
||||
fey = fabsf(e2.y());
|
||||
fez = fabsf(e2.z());
|
||||
AXISTEST_X2(e2.z(), e2.y(), fez, fey);
|
||||
AXISTEST_Y1(e2.z(), e2.x(), fez, fex);
|
||||
AXISTEST_Z12(e2.y(), e2.x(), fey, fex);
|
||||
|
||||
// Bullet 1:
|
||||
// first test overlap in the {x,y,z}-directions
|
||||
// find min, max of the triangle each direction, and test for overlap in
|
||||
// that direction -- this is equivalent to testing a minimal AABB around
|
||||
// the triangle against the AABB
|
||||
|
||||
// test in X-direction
|
||||
FINDMINMAX(v0.x(), v1.x(), v2.x(), min, max);
|
||||
if(min > boxhalfsize.x() || max < -boxhalfsize.x()) return false;
|
||||
|
||||
// test in Y-direction
|
||||
FINDMINMAX(v0.y(), v1.y(), v2.y(), min, max);
|
||||
if(min > boxhalfsize.y() || max < -boxhalfsize.y()) return false;
|
||||
|
||||
// test in Z-direction
|
||||
FINDMINMAX(v0.z(), v1.z(), v2.z(), min, max);
|
||||
if(min > boxhalfsize.z() || max < -boxhalfsize.z()) return false;
|
||||
|
||||
// Bullet 2:
|
||||
// test if the box intersects the plane of the triangle
|
||||
// compute plane equation of triangle: normal*x+d=0
|
||||
normal = cross(e0, e1);
|
||||
|
||||
return planeBoxOverlap(normal, v0, boxhalfsize);
|
||||
}
|
||||
|
||||
|
||||
bool nv::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:
|
||||
// 1) the {x,y,z}-directions (actually, since we use the AABB of the triangle
|
||||
// we do not even need to test these)
|
||||
// 2) normal of the triangle
|
||||
// 3) crossproduct(edge from tri, {x,y,z}-directin)
|
||||
// this gives 3x3=9 more tests
|
||||
Vector3 v0, v1, v2;
|
||||
float min, max, p0, p1, p2, rad, fex, fey, fez;
|
||||
Vector3 normal, e0, e1, e2;
|
||||
|
||||
// This is the fastest branch on Sun.
|
||||
// move everything so that the boxcenter is in (0,0,0)
|
||||
v0 = tri.v[0] - boxcenter;
|
||||
v1 = tri.v[1] - boxcenter;
|
||||
v2 = tri.v[2] - boxcenter;
|
||||
|
||||
// Compute triangle edges.
|
||||
e0 = v1 - v0; // tri edge 0
|
||||
e1 = v2 - v1; // tri edge 1
|
||||
e2 = v0 - v2; // tri edge 2
|
||||
|
||||
// Bullet 3:
|
||||
// test the 9 tests first (this was faster)
|
||||
fex = fabsf(e0.x());
|
||||
fey = fabsf(e0.y());
|
||||
fez = fabsf(e0.z());
|
||||
AXISTEST_X01(e0.z(), e0.y(), fez, fey);
|
||||
AXISTEST_Y02(e0.z(), e0.x(), fez, fex);
|
||||
AXISTEST_Z12(e0.y(), e0.x(), fey, fex);
|
||||
|
||||
fex = fabsf(e1.x());
|
||||
fey = fabsf(e1.y());
|
||||
fez = fabsf(e1.z());
|
||||
AXISTEST_X01(e1.z(), e1.y(), fez, fey);
|
||||
AXISTEST_Y02(e1.z(), e1.x(), fez, fex);
|
||||
AXISTEST_Z0(e1.y(), e1.x(), fey, fex);
|
||||
|
||||
fex = fabsf(e2.x());
|
||||
fey = fabsf(e2.y());
|
||||
fez = fabsf(e2.z());
|
||||
AXISTEST_X2(e2.z(), e2.y(), fez, fey);
|
||||
AXISTEST_Y1(e2.z(), e2.x(), fez, fex);
|
||||
AXISTEST_Z12(e2.y(), e2.x(), fey, fex);
|
||||
|
||||
// Bullet 2:
|
||||
// test if the box intersects the plane of the triangle
|
||||
// compute plane equation of triangle: normal*x+d=0
|
||||
normal = cross(e0, e1);
|
||||
|
||||
return planeBoxOverlap(normal, v0, boxhalfsize);
|
||||
}
|
||||
@ -1,168 +0,0 @@
|
||||
// This code is in the public domain -- Ignacio Casta<74>o <castanyo@yahoo.es>
|
||||
|
||||
#include <nvmath/Triangle.h>
|
||||
|
||||
using namespace nv;
|
||||
|
||||
|
||||
/// Tomas M<>ller, barycentric ray-triangle test.
|
||||
bool rayTest_Moller(const Triangle & t, Vector3::Arg orig, Vector3::Arg dir, float * out_t, float * out_u, float * out_v)
|
||||
{
|
||||
// find vectors for two edges sharing vert0
|
||||
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
|
||||
Vector3 pvec = cross(dir, e2);
|
||||
|
||||
// if determinant is near zero, ray lies in plane of triangle
|
||||
float det = dot(e1, pvec);
|
||||
if (det < -NV_EPSILON) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// calculate distance from vert0 to ray origin
|
||||
Vector3 tvec = orig - t.v[0];
|
||||
|
||||
// calculate U parameter and test bounds
|
||||
float u = dot(tvec, pvec);
|
||||
if( u < 0.0f || u > det ) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// prepare to test V parameter
|
||||
Vector3 qvec = cross(tvec, e1);
|
||||
|
||||
// calculate V parameter and test bounds
|
||||
float v = dot(dir, qvec);
|
||||
if (v < 0.0f || u + v > det) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// calculate t, scale parameters, ray intersects triangle
|
||||
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)
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#if 0
|
||||
|
||||
|
||||
// IC: This code is adapted from my Pi.MathLib code, based on Moller-Trumbore triangle test.
|
||||
FXVector3 edge1, edge2, pvec, tvec, qvec;
|
||||
|
||||
edge1 = tri.V1 - tri.V0;
|
||||
edge2 = tri.V2 - tri.V0;
|
||||
|
||||
pvec.Cross(ray.Direction, edge2);
|
||||
|
||||
float det = FXVector3.Dot(edge1, pvec);
|
||||
|
||||
// calculate distance from vert0 to ray origin.
|
||||
FXVector3 tvec = ray.Origin - vert0;
|
||||
|
||||
if( det < 0 )
|
||||
{
|
||||
// calculate U parameter and test bounds.
|
||||
float u = FXVector3.Dot(tvec, pvec);
|
||||
if (u > 0.0 || u < det)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// prepare to test V parameter.
|
||||
qvec.Cross(tvec, edge1);
|
||||
|
||||
// calculate V parameter and test bounds.
|
||||
float v = FXVector3.Dot(dir, qvec);
|
||||
|
||||
return v <= 0.0 && u + v >= det;
|
||||
}
|
||||
else
|
||||
{
|
||||
// calculate U parameter and test bounds.
|
||||
float u = FXVector3.Dot(tvec, pvec);
|
||||
if (u < 0.0 || u > det)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// prepare to test V parameter.
|
||||
qvec.Cross(tvec, edge1);
|
||||
|
||||
// calculate V parameter and test bounds.
|
||||
float v = FXVector3.Dot(dir, qvec);
|
||||
|
||||
return v >= 0.0 && u + v <= det;
|
||||
}
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* Dan Sunday, parametric ray-triangle test.
|
||||
*/
|
||||
// Output: *I = intersection point (when it exists)
|
||||
// Return: -1 = triangle is degenerate (a segment or point)
|
||||
// 0 = disjoint (no intersect)
|
||||
// 1 = intersect in unique point I1
|
||||
// 2 = are in the same plane
|
||||
bool RayTriangleTest( const Vec3 &p0, const Vec3 &p1,
|
||||
const Vec3 &v0, const Vec3 &v1, const Vec3 &v2, const Vec3 &n,
|
||||
Vec3 &I ) {
|
||||
Vec3 u, v; // triangle vectors
|
||||
Vec3 dir, w0, w; // ray vectors
|
||||
float r, a, b; // params to calc ray-plane intersect
|
||||
|
||||
// get triangle edge vectors and plane normal
|
||||
u.Sub( v1, v0 );
|
||||
v.Sub( v2, v0 );
|
||||
|
||||
dir.Sub( p1, p0 ); // ray direction vector
|
||||
w0.Sub( p0, v0 );
|
||||
a = Vec3DotProduct( n, w0 );
|
||||
b = Vec3DotProduct( n, dir );
|
||||
|
||||
if( fabs(b) < TI_EPSILON ) // ray is parallel to triangle plane
|
||||
return false;
|
||||
|
||||
|
||||
// get intersect point of ray with triangle plane
|
||||
r = -a / b;
|
||||
if( r < 0.0f ) // ray goes away from triangle
|
||||
return false; // => no intersect
|
||||
|
||||
// for a segment, also test if (r > 1.0) => no intersect
|
||||
|
||||
I.Mad( p0, dir, r ); // intersect point of ray and plane
|
||||
|
||||
// is I inside T?
|
||||
float uu, uv, vv, wu, wv, D;
|
||||
uu = Vec3DotProduct( u, u );
|
||||
uv = Vec3DotProduct( u, v );
|
||||
vv = Vec3DotProduct( v, v );
|
||||
w = I - v0;
|
||||
wu = Vec3DotProduct( w, u );
|
||||
wv = Vec3DotProduct( w, v );
|
||||
D = uv * uv - uu * vv;
|
||||
|
||||
// get and test parametric coords
|
||||
float s, t;
|
||||
s = (uv * wv - vv * wu) / D;
|
||||
if( s<0.0 || s > 1.0) // I is outside T
|
||||
return false;
|
||||
t = (uv * wu - uu * wv) / D;
|
||||
if( t<0.0 || (s + t) > 1.0) // I is outside T
|
||||
return false;
|
||||
|
||||
return true; // I is in T
|
||||
}
|
||||
|
||||
|
||||
#endif // 0
|
||||
@ -1,81 +0,0 @@
|
||||
// This code is in the public domain -- Ignacio Casta<74>o <castanyo@yahoo.es>
|
||||
|
||||
#ifndef NV_MATH_TRIANGLE_H
|
||||
#define NV_MATH_TRIANGLE_H
|
||||
|
||||
#include <nvmath/nvmath.h>
|
||||
#include <nvmath/Vector.h>
|
||||
#include <nvmath/Box.h>
|
||||
|
||||
namespace nv
|
||||
{
|
||||
|
||||
/// Triangle class with three vertices.
|
||||
class Triangle
|
||||
{
|
||||
public:
|
||||
Triangle() {};
|
||||
|
||||
Triangle(Vector3::Arg v0, Vector3::Arg v1, Vector3::Arg v2)
|
||||
{
|
||||
v[0] = v0;
|
||||
v[1] = v1;
|
||||
v[2] = v2;
|
||||
}
|
||||
|
||||
/// 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;
|
||||
}
|
||||
|
||||
Vector4 plane() const
|
||||
{
|
||||
Vector3 n = cross(v[1]-v[0], v[2]-v[0]);
|
||||
return Vector4(n, dot(n, v[0]));
|
||||
}
|
||||
|
||||
Vector3 v[3];
|
||||
};
|
||||
|
||||
|
||||
// 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.
|
||||
NVMATH_API bool rayTest_Moller(const Triangle & t, Vector3::Arg orig, Vector3::Arg dir, float * out_t, float * out_u, float * out_v);
|
||||
|
||||
inline bool rayTest(const Triangle & t, Vector3::Arg orig, Vector3::Arg dir, float * out_t, float * out_u, float * out_v)
|
||||
{
|
||||
return 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);
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
} // nv namespace
|
||||
|
||||
#endif // NV_MATH_TRIANGLE_H
|
||||
@ -1,87 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#include <nvcore/Stream.h>
|
||||
|
||||
#include <nvmath/Vector.h>
|
||||
#include <nvmath/Matrix.h>
|
||||
#include <nvmath/Quaternion.h>
|
||||
#include <nvmath/Basis.h>
|
||||
#include <nvmath/Box.h>
|
||||
#include <nvmath/Plane.h>
|
||||
|
||||
#include <nvmath/TypeSerialization.h>
|
||||
|
||||
using namespace nv;
|
||||
|
||||
Stream & nv::operator<< (Stream & s, Vector2 & v)
|
||||
{
|
||||
float x = v.x();
|
||||
float y = v.y();
|
||||
|
||||
s << x << y;
|
||||
|
||||
if (s.isLoading())
|
||||
{
|
||||
v.set(x, y);
|
||||
}
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
Stream & nv::operator<< (Stream & s, Vector3 & v)
|
||||
{
|
||||
float x = v.x();
|
||||
float y = v.y();
|
||||
float z = v.z();
|
||||
|
||||
s << x << y << z;
|
||||
|
||||
if (s.isLoading())
|
||||
{
|
||||
v.set(x, y, z);
|
||||
}
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
Stream & nv::operator<< (Stream & s, Vector4 & v)
|
||||
{
|
||||
float x = v.x();
|
||||
float y = v.y();
|
||||
float z = v.z();
|
||||
float w = v.w();
|
||||
|
||||
s << x << y << z << w;
|
||||
|
||||
if (s.isLoading())
|
||||
{
|
||||
v.set(x, y, z, w);
|
||||
}
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
Stream & nv::operator<< (Stream & s, Matrix & m)
|
||||
{
|
||||
return s;
|
||||
}
|
||||
|
||||
Stream & nv::operator<< (Stream & s, Quaternion & q)
|
||||
{
|
||||
return s << q.asVector();
|
||||
}
|
||||
|
||||
Stream & nv::operator<< (Stream & s, Basis & basis)
|
||||
{
|
||||
return s << basis.tangent << basis.bitangent << basis.normal;
|
||||
}
|
||||
|
||||
Stream & nv::operator<< (Stream & s, Box & box)
|
||||
{
|
||||
return s << box.m_mins << box.m_maxs;
|
||||
}
|
||||
|
||||
Stream & nv::operator<< (Stream & s, Plane & plane)
|
||||
{
|
||||
return s << plane.asVector();
|
||||
}
|
||||
@ -1,34 +0,0 @@
|
||||
// This code is in the public domain -- castanyo@yahoo.es
|
||||
|
||||
#ifndef NV_MATH_TYPESERIALIZATION_H
|
||||
#define NV_MATH_TYPESERIALIZATION_H
|
||||
|
||||
#include <nvmath/nvmath.h>
|
||||
|
||||
namespace nv
|
||||
{
|
||||
class Stream;
|
||||
|
||||
class Vector2;
|
||||
class Vector3;
|
||||
class Vector4;
|
||||
|
||||
class Matrix;
|
||||
class Quaternion;
|
||||
class Basis;
|
||||
class Box;
|
||||
class Plane;
|
||||
|
||||
NVMATH_API Stream & operator<< (Stream & s, Vector2 & obj);
|
||||
NVMATH_API Stream & operator<< (Stream & s, Vector3 & obj);
|
||||
NVMATH_API Stream & operator<< (Stream & s, Vector4 & obj);
|
||||
|
||||
NVMATH_API Stream & operator<< (Stream & s, Matrix & obj);
|
||||
NVMATH_API Stream & operator<< (Stream & s, Quaternion & obj);
|
||||
NVMATH_API Stream & operator<< (Stream & s, Basis & obj);
|
||||
NVMATH_API Stream & operator<< (Stream & s, Box & obj);
|
||||
NVMATH_API Stream & operator<< (Stream & s, Plane & obj);
|
||||
|
||||
} // nv namespace
|
||||
|
||||
#endif // NV_MATH_TYPESERIALIZATION_H
|
||||
@ -48,19 +48,37 @@
|
||||
#define IS_NEGATIVE_FLOAT(x) (IR(x)&SIGN_BITMASK)
|
||||
*/
|
||||
|
||||
inline float sqrt_assert(const float f)
|
||||
inline double sqrt_assert(const double f)
|
||||
{
|
||||
nvDebugCheck(f >= 0.0f);
|
||||
return sqrt(f);
|
||||
}
|
||||
|
||||
inline float sqrtf_assert(const float f)
|
||||
{
|
||||
nvDebugCheck(f >= 0.0f);
|
||||
return sqrtf(f);
|
||||
}
|
||||
|
||||
inline float acos_assert(const float f)
|
||||
inline double acos_assert(const double f)
|
||||
{
|
||||
nvDebugCheck(f >= -1.0f && f <= 1.0f);
|
||||
return acos(f);
|
||||
}
|
||||
|
||||
inline float acosf_assert(const float f)
|
||||
{
|
||||
nvDebugCheck(f >= -1.0f && f <= 1.0f);
|
||||
return acosf(f);
|
||||
}
|
||||
|
||||
inline float asin_assert(const float f)
|
||||
inline double asin_assert(const double f)
|
||||
{
|
||||
nvDebugCheck(f >= -1.0f && f <= 1.0f);
|
||||
return asin(f);
|
||||
}
|
||||
|
||||
inline float asinf_assert(const float f)
|
||||
{
|
||||
nvDebugCheck(f >= -1.0f && f <= 1.0f);
|
||||
return asinf(f);
|
||||
@ -68,11 +86,11 @@ inline float asin_assert(const float f)
|
||||
|
||||
// Replace default functions with asserting ones.
|
||||
#define sqrt sqrt_assert
|
||||
#define sqrtf sqrt_assert
|
||||
#define sqrtf sqrtf_assert
|
||||
#define acos acos_assert
|
||||
#define acosf acos_assert
|
||||
#define acosf acosf_assert
|
||||
#define asin asin_assert
|
||||
#define asinf asin_assert
|
||||
#define asinf asinf_assert
|
||||
|
||||
#if NV_OS_WIN32
|
||||
#include <float.h>
|
||||
|
||||
Reference in New Issue
Block a user