kookroach/game-physics
1
0
Fork 0
Code Issues 8 Pull Requests Packages Projects 1 Releases Wiki Activity

Ex3

Browse Source
This commit is contained in:
youxie
2019-12-01 22:57:43 +01:00
parent 56a95ec733
commit 2324790537
3 changed files with 160 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

750
Simulations/util/pcgsolver.h
View File

@@ -1,750 +0,0 @@
//
// Preconditioned conjugate gradient solver
//
// Created by Robert Bridson, Ryoichi Ando and Nils Thuerey
//
#ifndef RCMATRIX3_H
#define RCMATRIX3_H
#include <iterator>
#include <cassert>
#include <vector>
#include <fstream>
#include <cmath>
#include <functional>
// index type
#define int_index long long
// parallelization disabled
#define parallel_for(size) { int thread_number = 0; int_index parallel_index=0; for( int_index parallel_index=0; parallel_index<(int_index)size; parallel_index++ ) {
#define parallel_end } thread_number=parallel_index=0; }
#define parallel_block
#define do_parallel
#define do_end
#define block_end
#include "vectorbase.h"
// note - "Int" instead of "N" here, the latter is size!
template<class Int, class T>
struct InstantBLAS {
static inline Int offset(Int N, Int incX) { return ((incX) > 0 ? 0 : ((N) - 1) * (-(incX))); }
static T cblas_ddot( const Int N, const T *X, const Int incX, const T *Y, const Int incY) {
double r = 0.0; // always use double precision internally here...
Int i;
Int ix = offset(N,incX);
Int iy = offset(N,incY);
for (i = 0; i < N; i++) {
r += X[ix] * Y[iy];
ix += incX;
iy += incY;
}
return (T)r;
}
static void cblas_daxpy( const Int N, const T alpha, const T *X, const Int incX, T *Y, const Int incY) {
Int i;
if (N <= 0 ) return;
if (alpha == 0.0) return;
if (incX == 1 && incY == 1) {
const Int m = N % 4;
for (i = 0; i < m; i++)
Y[i] += alpha * X[i];
for (i = m; i + 3 < N; i += 4) {
Y[i ] += alpha * X[i ];
Y[i + 1] += alpha * X[i + 1];
Y[i + 2] += alpha * X[i + 2];
Y[i + 3] += alpha * X[i + 3];
}
} else {
Int ix = offset(N, incX);
Int iy = offset(N, incY);
for (i = 0; i < N; i++) {
Y[iy] += alpha * X[ix];
ix += incX;
iy += incY;
}
}
}
// dot products ==============================================================
static inline T dot(const std::vector<T> &x, const std::vector<T> &y) {
return cblas_ddot((int)x.size(), &x[0], 1, &y[0], 1);
}
// inf-norm (maximum absolute value: index of max returned) ==================
static inline Int index_abs_max(const std::vector<T> &x) {
int maxind = 0;
T maxvalue = 0;
for(Int i = 0; i < (Int)x.size(); ++i) {
if(std::abs(x[i]) > maxvalue) {
maxvalue = fabs(x[i]);
maxind = i;
}
}
return maxind;
}
// inf-norm (maximum absolute value) =========================================
// technically not part of BLAS, but useful
static inline T abs_max(const std::vector<T> &x)
{ return std::abs(x[index_abs_max(x)]); }
// saxpy (y=alpha*x+y) =======================================================
static inline void add_scaled(T alpha, const std::vector<T> &x, std::vector<T> &y) {
cblas_daxpy((Int)x.size(), alpha, &x[0], 1, &y[0], 1);
}
};
template<class T>
void zero(std::vector<T> &v)
{ for(int i=(int)v.size()-1; i>=0; --i) v[i]=0; }
template<class T>
void insert(std::vector<T> &a, unsigned int index, T e)
{
a.push_back(a.back());
for(unsigned int i=(unsigned int)a.size()-1; i>index; --i)
a[i]=a[i-1];
a[index]=e;
}
template<class T>
void erase(std::vector<T> &a, unsigned int index)
{
for(unsigned int i=index; i<a.size()-1; ++i)
a[i]=a[i+1];
a.pop_back();
}
//============================================================================
// Dynamic compressed sparse row matrix.
template<class T>
struct SparseMatrix
{
int n; // dimension
std::vector<std::vector<int> > index; // for each row, a list of all column indices (sorted)
std::vector<std::vector<T> > value; // values corresponding to index
explicit SparseMatrix(int n_=0, int expected_nonzeros_per_row=7)
: n(n_), index(n_), value(n_)
{
for(int i=0; i<n; ++i){
index[i].reserve(expected_nonzeros_per_row);
value[i].reserve(expected_nonzeros_per_row);
}
}
void clear(void)
{
n=0;
index.clear();
value.clear();
}
void zero(void)
{
for(int i=0; i<n; ++i){
index[i].resize(0);
value[i].resize(0);
}
}
void resize(int n_)
{
n=n_;
index.resize(n);
value.resize(n);
}
T operator()(int i, int j) const
{
for(int k=0; k<(int)index[i].size(); ++k){
if(index[i][k]==j) return value[i][k];
else if(index[i][k]>j) return 0;
}
return 0;
}
void set_element(int i, int j, T new_value)
{
int k=0;
for(; k<(int)index[i].size(); ++k){
if(index[i][k]==j){
value[i][k]=new_value;
return;
}else if(index[i][k]>j){
insert(index[i], k, j);
insert(value[i], k, new_value);
return;
}
}
index[i].push_back(j);
value[i].push_back(new_value);
}
void add_to_element(int i, int j, T increment_value)
{
int k=0;
for(; k<(int)index[i].size(); ++k){
if(index[i][k]==j){
value[i][k]+=increment_value;
return;
}else if(index[i][k]>j){
insert(index[i], k, j);
insert(value[i], k, increment_value);
return;
}
}
index[i].push_back(j);
value[i].push_back(increment_value);
}
// assumes indices is already sorted
void add_sparse_row(int i, const std::vector<int> &indices, const std::vector<T> &values)
{
int j=0, k=0;
while(j<indices.size() && k<(int)index[i].size()){
if(index[i][k]<indices[j]){
++k;
}else if(index[i][k]>indices[j]){
insert(index[i], k, indices[j]);
insert(value[i], k, values[j]);
++j;
}else{
value[i][k]+=values[j];
++j;
++k;
}
}
for(;j<indices.size(); ++j){
index[i].push_back(indices[j]);
value[i].push_back(values[j]);
}
}
// assumes matrix has symmetric structure - so the indices in row i tell us which columns to delete i from
void symmetric_remove_row_and_column(int i)
{
for(int a=0; a<index[i].size(); ++a){
int j=index[i][a]; //
for(int b=0; b<index[j].size(); ++b){
if(index[j][b]==i){
erase(index[j], b);
erase(value[j], b);
break;
}
}
}
index[i].resize(0);
value[i].resize(0);
}
void write_matlab(std::ostream &output, const char *variable_name)
{
output<<variable_name<<"=sparse([";
for(int i=0; i<n; ++i){
for(int j=0; j<index[i].size(); ++j){
output<<i+1<<" ";
}
}
output<<"],...\n [";
for(int i=0; i<n; ++i){
for(int j=0; j<index[i].size(); ++j){
output<<index[i][j]+1<<" ";
}
}
output<<"],...\n [";
for(int i=0; i<n; ++i){
for(int j=0; j<value[i].size(); ++j){
output<<value[i][j]<<" ";
}
}
output<<"], "<<n<<", "<<n<<");"<<std::endl;
}
};
typedef SparseMatrix<float> SparseMatrixf;
typedef SparseMatrix<double> SparseMatrixd;
// perform result=matrix*x
template<class T>
void multiply(const SparseMatrix<T> &matrix, const std::vector<T> &x, std::vector<T> &result)
{
assert(matrix.n==x.size());
result.resize(matrix.n);
//for(int i=0; i<matrix.n; ++i)
parallel_for(matrix.n) {
unsigned i (parallel_index);
T value=0;
for(int j=0; j<(int)matrix.index[i].size(); ++j){
value+=matrix.value[i][j]*x[matrix.index[i][j]];
}
result[i]=value;
} parallel_end
}
// perform result=result-matrix*x
template<class T>
void multiply_and_subtract(const SparseMatrix<T> &matrix, const std::vector<T> &x, std::vector<T> &result)
{
assert(matrix.n==x.size());
result.resize(matrix.n);
for(int i=0; i<(int)matrix.n; ++i){
for(int j=0; j<(int)matrix.index[i].size(); ++j){
result[i]-=matrix.value[i][j]*x[matrix.index[i][j]];
}
}
}
//============================================================================
// Fixed version of SparseMatrix. This is not a good structure for dynamically
// modifying the matrix, but can be significantly faster for matrix-vector
// multiplies due to better data locality.
template<class T>
struct FixedSparseMatrix
{
int n; // dimension
std::vector<T> value; // nonzero values row by row
std::vector<int> colindex; // corresponding column indices
std::vector<int> rowstart; // where each row starts in value and colindex (and last entry is one past the end, the number of nonzeros)
explicit FixedSparseMatrix(int n_=0)
: n(n_), value(0), colindex(0), rowstart(n_+1)
{}
void clear(void)
{
n=0;
value.clear();
colindex.clear();
rowstart.clear();
}
void resize(int n_)
{
n=n_;
rowstart.resize(n+1);
}
void construct_from_matrix(const SparseMatrix<T> &matrix)
{
resize(matrix.n);
rowstart[0]=0;
for(int i=0; i<n; ++i){
rowstart[i+1]=rowstart[i]+matrix.index[i].size();
}
value.resize(rowstart[n]);
colindex.resize(rowstart[n]);
int j=0;
for(int i=0; i<n; ++i){
for(int k=0; k<(int)matrix.index[i].size(); ++k){
value[j]=matrix.value[i][k];
colindex[j]=matrix.index[i][k];
++j;
}
}
}
void write_matlab(std::ostream &output, const char *variable_name)
{
output<<variable_name<<"=sparse([";
for(int i=0; i<n; ++i){
for(int j=rowstart[i]; j<rowstart[i+1]; ++j){
output<<i+1<<" ";
}
}
output<<"],...\n [";
for(int i=0; i<n; ++i){
for(int j=rowstart[i]; j<rowstart[i+1]; ++j){
output<<colindex[j]+1<<" ";
}
}
output<<"],...\n [";
for(int i=0; i<n; ++i){
for(int j=rowstart[i]; j<rowstart[i+1]; ++j){
output<<value[j]<<" ";
}
}
output<<"], "<<n<<", "<<n<<");"<<std::endl;
}
};
// perform result=matrix*x
template<class T>
void multiply(const FixedSparseMatrix<T> &matrix, const std::vector<T> &x, std::vector<T> &result)
{
assert(matrix.n==x.size());
result.resize(matrix.n);
//for(int i=0; i<matrix.n; ++i)
parallel_for(matrix.n) {
unsigned i (parallel_index);
T value=0;
for(int j=matrix.rowstart[i]; j<matrix.rowstart[i+1]; ++j){
value+=matrix.value[j]*x[matrix.colindex[j]];
}
result[i]=value;
} parallel_end
}
// perform result=result-matrix*x
template<class T>
void multiply_and_subtract(const FixedSparseMatrix<T> &matrix, const std::vector<T> &x, std::vector<T> &result)
{
assert(matrix.n==x.size());
result.resize(matrix.n);
for(int i=0; i<matrix.n; ++i){
for(int j=matrix.rowstart[i]; j<matrix.rowstart[i+1]; ++j){
result[i]-=matrix.value[j]*x[matrix.colindex[j]];
}
}
}
//============================================================================
// A simple compressed sparse column data structure (with separate diagonal)
// for lower triangular matrices
template<class T>
struct SparseColumnLowerFactor
{
int n;
std::vector<T> invdiag; // reciprocals of diagonal elements
std::vector<T> value; // values below the diagonal, listed column by column
std::vector<int> rowindex; // a list of all row indices, for each column in turn
std::vector<int> colstart; // where each column begins in rowindex (plus an extra entry at the end, of #nonzeros)
std::vector<T> adiag; // just used in factorization: minimum "safe" diagonal entry allowed
explicit SparseColumnLowerFactor(int n_=0)
: n(n_), invdiag(n_), colstart(n_+1), adiag(n_)
{}
void clear(void)
{
n=0;
invdiag.clear();
value.clear();
rowindex.clear();
colstart.clear();
adiag.clear();
}
void resize(int n_)
{
n=n_;
invdiag.resize(n);
colstart.resize(n+1);
adiag.resize(n);
}
void write_matlab(std::ostream &output, const char *variable_name)
{
output<<variable_name<<"=sparse([";
for(int i=0; i<n; ++i){
output<<" "<<i+1;
for(int j=colstart[i]; j<colstart[i+1]; ++j){
output<<" "<<rowindex[j]+1;
}
}
output<<"],...\n [";
for(int i=0; i<n; ++i){
output<<" "<<i+1;
for(int j=colstart[i]; j<colstart[i+1]; ++j){
output<<" "<<i+1;
}
}
output<<"],...\n [";
for(int i=0; i<n; ++i){
output<<" "<<(invdiag[i]!=0 ? 1/invdiag[i] : 0);
for(int j=colstart[i]; j<colstart[i+1]; ++j){
output<<" "<<value[j];
}
}
output<<"], "<<n<<", "<<n<<");"<<std::endl;
}
};
//============================================================================
// Incomplete Cholesky factorization, level zero, with option for modified version.
// Set modification_parameter between zero (regular incomplete Cholesky) and
// one (fully modified version), with values close to one usually giving the best
// results. The min_diagonal_ratio parameter is used to detect and correct
// problems in factorization: if a pivot is this much less than the diagonal
// entry from the original matrix, the original matrix entry is used instead.
template<class T>
void factor_modified_incomplete_cholesky0(const SparseMatrix<T> &matrix, SparseColumnLowerFactor<T> &factor,
T modification_parameter=0.97, T min_diagonal_ratio=0.25)
{
// first copy lower triangle of matrix into factor (Note: assuming A is symmetric of course!)
factor.resize(matrix.n);
zero(factor.invdiag); // important: eliminate old values from previous solves!
factor.value.resize(0);
factor.rowindex.resize(0);
zero(factor.adiag);
for(int i=0; i<matrix.n; ++i){
factor.colstart[i]=(int)factor.rowindex.size();
for(int j=0; j<(int)matrix.index[i].size(); ++j){
if(matrix.index[i][j]>i){
factor.rowindex.push_back(matrix.index[i][j]);
factor.value.push_back(matrix.value[i][j]);
}else if(matrix.index[i][j]==i){
factor.invdiag[i]=factor.adiag[i]=matrix.value[i][j];
}
}
}
factor.colstart[matrix.n]=(int)factor.rowindex.size();
// now do the incomplete factorization (figure out numerical values)
// MATLAB code:
// L=tril(A);
// for k=1:size(L,2)
// L(k,k)=sqrt(L(k,k));
// L(k+1:end,k)=L(k+1:end,k)/L(k,k);
// for j=find(L(:,k))'
// if j>k
// fullupdate=L(:,k)*L(j,k);
// incompleteupdate=fullupdate.*(A(:,j)~=0);
// missing=sum(fullupdate-incompleteupdate);
// L(j:end,j)=L(j:end,j)-incompleteupdate(j:end);
// L(j,j)=L(j,j)-omega*missing;
// end
// end
// end
for(int k=0; k<matrix.n; ++k){
if(factor.adiag[k]==0) continue; // null row/column
// figure out the final L(k,k) entry
if(factor.invdiag[k]<min_diagonal_ratio*factor.adiag[k])
factor.invdiag[k]=1/sqrt(factor.adiag[k]); // drop to Gauss-Seidel here if the pivot looks dangerously small
else
factor.invdiag[k]=1/sqrt(factor.invdiag[k]);
// finalize the k'th column L(:,k)
for(int p=factor.colstart[k]; p<factor.colstart[k+1]; ++p){
factor.value[p]*=factor.invdiag[k];
}
// incompletely eliminate L(:,k) from future columns, modifying diagonals
for(int p=factor.colstart[k]; p<factor.colstart[k+1]; ++p){
int j=factor.rowindex[p]; // work on column j
T multiplier=factor.value[p];
T missing=0;
int a=factor.colstart[k];
// first look for contributions to missing from dropped entries above the diagonal in column j
int b=0;
while(a<factor.colstart[k+1] && factor.rowindex[a]<j){
// look for factor.rowindex[a] in matrix.index[j] starting at b
while(b<(int)matrix.index[j].size()){
if(matrix.index[j][b]<factor.rowindex[a])
++b;
else if(matrix.index[j][b]==factor.rowindex[a])
break;
else{
missing+=factor.value[a];
break;
}
}
++a;
}
// adjust the diagonal j,j entry
if(a<factor.colstart[k+1] && factor.rowindex[a]==j){
factor.invdiag[j]-=multiplier*factor.value[a];
}
++a;
// and now eliminate from the nonzero entries below the diagonal in column j (or add to missing if we can't)
b=factor.colstart[j];
while(a<factor.colstart[k+1] && b<factor.colstart[j+1]){
if(factor.rowindex[b]<factor.rowindex[a])
++b;
else if(factor.rowindex[b]==factor.rowindex[a]){
factor.value[b]-=multiplier*factor.value[a];
++a;
++b;
}else{
missing+=factor.value[a];
++a;
}
}
// and if there's anything left to do, add it to missing
while(a<factor.colstart[k+1]){
missing+=factor.value[a];
++a;
}
// and do the final diagonal adjustment from the missing entries
factor.invdiag[j]-=modification_parameter*multiplier*missing;
}
}
}
//============================================================================
// Solution routines with lower triangular matrix.
// solve L*result=rhs
template<class T>
void solve_lower(const SparseColumnLowerFactor<T> &factor, const std::vector<T> &rhs, std::vector<T> &result)
{
assert(factor.n==rhs.size());
assert(factor.n==result.size());
result=rhs;
for(int i=0; i<factor.n; ++i){
result[i]*=factor.invdiag[i];
for(int j=factor.colstart[i]; j<factor.colstart[i+1]; ++j){
result[factor.rowindex[j]]-=factor.value[j]*result[i];
}
}
}
// solve L^T*result=rhs
template<class T>
void solve_lower_transpose_in_place(const SparseColumnLowerFactor<T> &factor, std::vector<T> &x)
{
assert(factor.n==(int)x.size());
assert(factor.n>0);
int i=factor.n;
do{
--i;
for(int j=factor.colstart[i]; j<factor.colstart[i+1]; ++j){
x[i]-=factor.value[j]*x[factor.rowindex[j]];
}
x[i]*=factor.invdiag[i];
}while(i!=0);
}
//============================================================================
// Encapsulates the Conjugate Gradient algorithm with incomplete Cholesky
// factorization preconditioner.
template <class T>
struct SparsePCGSolver
{
SparsePCGSolver(void)
{
set_solver_parameters(1e-5, 100, 0.97, 0.25);
}
void set_solver_parameters(T tolerance_factor_, int max_iterations_, T modified_incomplete_cholesky_parameter_=0.97, T min_diagonal_ratio_=0.25)
{
tolerance_factor=tolerance_factor_;
if(tolerance_factor<1e-30) tolerance_factor=1e-30;
max_iterations=max_iterations_;
modified_incomplete_cholesky_parameter=modified_incomplete_cholesky_parameter_;
min_diagonal_ratio=min_diagonal_ratio_;
}
bool solve(const SparseMatrix<T> &matrix, const std::vector<T> &rhs, std::vector<T> &result, T &relative_residual_out, int &iterations_out, int precondition=2)
{
int n=matrix.n;
if((int)m.size()!=n){ m.resize(n); s.resize(n); z.resize(n); r.resize(n); }
zero(result);
r=rhs;
double residual_out=InstantBLAS<int,T>::abs_max(r);
if(residual_out==0) {
iterations_out=0;
return true;
}
//double tol=tolerance_factor*residual_out; // relative residual
double tol=tolerance_factor;
double residual_0 = residual_out;
form_preconditioner(matrix, precondition);
apply_preconditioner( r, z, precondition);
double rho=InstantBLAS<int,T>::dot(z, r);
if(rho==0 || rho!=rho) {
iterations_out=0;
return false;
}
s=z;
fixed_matrix.construct_from_matrix(matrix);
int iteration;
for(iteration=0; iteration<max_iterations; ++iteration){
multiply(fixed_matrix, s, z);
double alpha=rho/InstantBLAS<int,T>::dot(s, z);
InstantBLAS<int,T>::add_scaled(alpha, s, result);
InstantBLAS<int,T>::add_scaled(-alpha, z, r);
residual_out=InstantBLAS<int,T>::abs_max(r);
relative_residual_out = residual_out / residual_0;
if(residual_out<=tol) {
iterations_out=iteration+1;
return true;
}
apply_preconditioner(r, z, precondition);
double rho_new=InstantBLAS<int,T>::dot(z, r);
double beta=rho_new/rho;
InstantBLAS<int,T>::add_scaled(beta, s, z); s.swap(z); // s=beta*s+z
rho=rho_new;
}
iterations_out=iteration;
relative_residual_out = residual_out / residual_0;
return false;
}
protected:
// internal structures
SparseColumnLowerFactor<T> ic_factor; // modified incomplete cholesky factor
std::vector<T> m, z, s, r; // temporary vectors for PCG
FixedSparseMatrix<T> fixed_matrix; // used within loop
// parameters
T tolerance_factor;
int max_iterations;
T modified_incomplete_cholesky_parameter;
T min_diagonal_ratio;
void form_preconditioner(const SparseMatrix<T>& matrix, int precondition=2)
{
if(precondition==2) {
// incomplete cholesky
factor_modified_incomplete_cholesky0(matrix, ic_factor, modified_incomplete_cholesky_parameter, min_diagonal_ratio);
} else if(precondition==1) {
// diagonal
ic_factor.resize(matrix.n);
zero(ic_factor.invdiag);
for(int i=0; i<matrix.n; ++i) {
for(int j=0; j<(int)matrix.index[i].size(); ++j){
if(matrix.index[i][j]==i){
ic_factor.invdiag[i] = 1./matrix.value[i][j];
}
}
}
}
}
void apply_preconditioner(const std::vector<T> &x, std::vector<T> &result, int precondition=2)
{
if (precondition==2) {
// incomplete cholesky
solve_lower(ic_factor, x, result);
solve_lower_transpose_in_place(ic_factor,result);
} else if(precondition==1) {
// diagonal
for(int_index i=0; i<(int_index)result.size(); ++i) {
result[i] = x[i] * ic_factor.invdiag[i];
}
} else {
// off
result = x;
}
}
};
#undef parallel_for
#undef parallel_end
#undef int_index
#undef parallel_block
#undef do_parallel
#undef do_end
#undef block_end
#endif

611
Simulations/util/vectorbase.h
View File

@@ -1,611 +0,0 @@
/******************************************************************************
*
* MantaFlow fluid solver framework
* Copyright 2011-2016 Tobias Pfaff, Nils Thuerey
*
* This program is free software, distributed under the terms of the
* GNU General Public License (GPL)
* http://www.gnu.org/licenses
*
* Basic vector class
*
******************************************************************************/
#ifndef _VECTORBASE_H
#define _VECTORBASE_H
// get rid of windos min/max defines
#if defined(WIN32) || defined(_WIN32)
# define NOMINMAX
#endif
#include <stdio.h>
#include <string>
#include <limits>
#include <iostream>
#include "general.h"
// if min/max are still around...
#if defined(WIN32) || defined(_WIN32)
# undef min
# undef max
#endif
// redefine usage of some windows functions
#if defined(WIN32) || defined(_WIN32)
# ifndef snprintf
# define snprintf _snprintf
# endif
#endif
// use which fp-precision? 1=float, 2=double
#ifndef FLOATINGPOINT_PRECISION
# define FLOATINGPOINT_PRECISION 1
#endif
// VECTOR_EPSILON is the minimal vector length
// In order to be able to discriminate floating point values near zero, and
// to be sure not to fail a comparison because of roundoff errors, use this
// value as a threshold.
#if FLOATINGPOINT_PRECISION==1
typedef float Real;
# define VECTOR_EPSILON (1e-6f)
#else
typedef double Real;
# define VECTOR_EPSILON (1e-10)
#endif
#ifndef M_PI
# define M_PI 3.1415926536
#endif
#ifndef M_E
# define M_E 2.7182818284
#endif
namespace Manta
{
//! Basic inlined vector class
template<class S>
class Vector3D
{
public:
//! Constructor
inline Vector3D() : x(0),y(0),z(0) {}
//! Copy-Constructor
inline Vector3D ( const Vector3D<S> &v ) : x(v.x), y(v.y), z(v.z) {}
//! Copy-Constructor
inline Vector3D ( const float * v) : x((S)v[0]), y((S)v[1]), z((S)v[2]) {}
//! Copy-Constructor
inline Vector3D ( const double * v) : x((S)v[0]), y((S)v[1]), z((S)v[2]) {}
//! Construct a vector from one S
inline Vector3D ( S v) : x(v), y(v), z(v) {}
//! Construct a vector from three Ss
inline Vector3D ( S vx, S vy, S vz) : x(vx), y(vy), z(vz) {}
// Operators
//! Assignment operator
inline const Vector3D<S>& operator= ( const Vector3D<S>& v ) {
x = v.x;
y = v.y;
z = v.z;
return *this;
}
//! Assignment operator
inline const Vector3D<S>& operator= ( S s ) {
x = y = z = s;
return *this;
}
//! Assign and add operator
inline const Vector3D<S>& operator+= ( const Vector3D<S>& v ) {
x += v.x;
y += v.y;
z += v.z;
return *this;
}
//! Assign and add operator
inline const Vector3D<S>& operator+= ( S s ) {
x += s;
y += s;
z += s;
return *this;
}
//! Assign and sub operator
inline const Vector3D<S>& operator-= ( const Vector3D<S>& v ) {
x -= v.x;
y -= v.y;
z -= v.z;
return *this;
}
//! Assign and sub operator
inline const Vector3D<S>& operator-= ( S s ) {
x -= s;
y -= s;
z -= s;
return *this;
}
//! Assign and mult operator
inline const Vector3D<S>& operator*= ( const Vector3D<S>& v ) {
x *= v.x;
y *= v.y;
z *= v.z;
return *this;
}
//! Assign and mult operator
inline const Vector3D<S>& operator*= ( S s ) {
x *= s;
y *= s;
z *= s;
return *this;
}
//! Assign and div operator
inline const Vector3D<S>& operator/= ( const Vector3D<S>& v ) {
x /= v.x;
y /= v.y;
z /= v.z;
return *this;
}
//! Assign and div operator
inline const Vector3D<S>& operator/= ( S s ) {
x /= s;
y /= s;
z /= s;
return *this;
}
//! Negation operator
inline Vector3D<S> operator- () const {
return Vector3D<S> (-x, -y, -z);
}
//! Get smallest component
inline S min() const {
return ( x<y ) ? ( ( x<z ) ? x:z ) : ( ( y<z ) ? y:z );
}
//! Get biggest component
inline S max() const {
return ( x>y ) ? ( ( x>z ) ? x:z ) : ( ( y>z ) ? y:z );
}
//! Test if all components are zero
inline bool empty() {
return x==0 && y==0 && z==0;
}
//! access operator
inline S& operator[] ( unsigned int i ) {
return value[i];
}
//! constant access operator
inline const S& operator[] ( unsigned int i ) const {
return value[i];
}
//! debug output vector to a string
std::string toString() const;
//! test if nans are present
bool isValid() const;
//! actual values
union {
S value[3];
struct {
S x;
S y;
S z;
};
struct {
S X;
S Y;
S Z;
};
};
//! zero element
static const Vector3D<S> Zero, Invalid;
//! For compatibility with 4d vectors (discards 4th comp)
inline Vector3D ( S vx, S vy, S vz, S vDummy) : x(vx), y(vy), z(vz) {}
protected:
};
//************************************************************************
// Additional operators
//************************************************************************
//! Addition operator
template<class S>
inline Vector3D<S> operator+ ( const Vector3D<S> &v1, const Vector3D<S> &v2 ) {
return Vector3D<S> ( v1.x+v2.x, v1.y+v2.y, v1.z+v2.z );
}
//! Addition operator
template<class S, class S2>
inline Vector3D<S> operator+ ( const Vector3D<S>& v, S2 s ) {
return Vector3D<S> ( v.x+s, v.y+s, v.z+s );
}
//! Addition operator
template<class S, class S2>
inline Vector3D<S> operator+ ( S2 s, const Vector3D<S>& v ) {
return Vector3D<S> ( v.x+s, v.y+s, v.z+s );
}
//! Subtraction operator
template<class S>
inline Vector3D<S> operator- ( const Vector3D<S> &v1, const Vector3D<S> &v2 ) {
return Vector3D<S> ( v1.x-v2.x, v1.y-v2.y, v1.z-v2.z );
}
//! Subtraction operator
template<class S, class S2>
inline Vector3D<S> operator- ( const Vector3D<S>& v, S2 s ) {
return Vector3D<S> ( v.x-s, v.y-s, v.z-s );
}
//! Subtraction operator
template<class S, class S2>
inline Vector3D<S> operator- ( S2 s, const Vector3D<S>& v ) {
return Vector3D<S> ( s-v.x, s-v.y, s-v.z );
}
//! Multiplication operator
template<class S>
inline Vector3D<S> operator* ( const Vector3D<S> &v1, const Vector3D<S> &v2 ) {
return Vector3D<S> ( v1.x*v2.x, v1.y*v2.y, v1.z*v2.z );
}
//! Multiplication operator
template<class S, class S2>
inline Vector3D<S> operator* ( const Vector3D<S>& v, S2 s ) {
return Vector3D<S> ( v.x*s, v.y*s, v.z*s );
}
//! Multiplication operator
template<class S, class S2>
inline Vector3D<S> operator* ( S2 s, const Vector3D<S>& v ) {
return Vector3D<S> ( s*v.x, s*v.y, s*v.z );
}
//! Division operator
template<class S>
inline Vector3D<S> operator/ ( const Vector3D<S> &v1, const Vector3D<S> &v2 ) {
return Vector3D<S> ( v1.x/v2.x, v1.y/v2.y, v1.z/v2.z );
}
//! Division operator
template<class S, class S2>
inline Vector3D<S> operator/ ( const Vector3D<S>& v, S2 s ) {
return Vector3D<S> ( v.x/s, v.y/s, v.z/s );
}
//! Division operator
template<class S, class S2>
inline Vector3D<S> operator/ ( S2 s, const Vector3D<S>& v ) {
return Vector3D<S> ( s/v.x, s/v.y, s/v.z );
}
//! Comparison operator
template<class S>
inline bool operator== (const Vector3D<S>& s1, const Vector3D<S>& s2) {
return s1.x == s2.x && s1.y == s2.y && s1.z == s2.z;
}
//! Comparison operator
template<class S>
inline bool operator!= (const Vector3D<S>& s1, const Vector3D<S>& s2) {
return s1.x != s2.x || s1.y != s2.y || s1.z != s2.z;
}
//************************************************************************
// External functions
//************************************************************************
//! Min operator
template<class S>
inline Vector3D<S> vmin (const Vector3D<S>& s1, const Vector3D<S>& s2) {
return Vector3D<S>(std::min(s1.x,s2.x), std::min(s1.y,s2.y), std::min(s1.z,s2.z));
}
//! Min operator
template<class S, class S2>
inline Vector3D<S> vmin (const Vector3D<S>& s1, S2 s2) {
return Vector3D<S>(std::min(s1.x,s2), std::min(s1.y,s2), std::min(s1.z,s2));
}
//! Min operator
template<class S1, class S>
inline Vector3D<S> vmin (S1 s1, const Vector3D<S>& s2) {
return Vector3D<S>(std::min(s1,s2.x), std::min(s1,s2.y), std::min(s1,s2.z));
}
//! Max operator
template<class S>
inline Vector3D<S> vmax (const Vector3D<S>& s1, const Vector3D<S>& s2) {
return Vector3D<S>(std::max(s1.x,s2.x), std::max(s1.y,s2.y), std::max(s1.z,s2.z));
}
//! Max operator
template<class S, class S2>
inline Vector3D<S> vmax (const Vector3D<S>& s1, S2 s2) {
return Vector3D<S>(std::max(s1.x,s2), std::max(s1.y,s2), std::max(s1.z,s2));
}
//! Max operator
template<class S1, class S>
inline Vector3D<S> vmax (S1 s1, const Vector3D<S>& s2) {
return Vector3D<S>(std::max(s1,s2.x), std::max(s1,s2.y), std::max(s1,s2.z));
}
//! Dot product
template<class S>
inline S dot ( const Vector3D<S> &t, const Vector3D<S> &v ) {
return t.x*v.x + t.y*v.y + t.z*v.z;
}
//! Cross product
template<class S>
inline Vector3D<S> cross ( const Vector3D<S> &t, const Vector3D<S> &v ) {
Vector3D<S> cp (
( ( t.y*v.z ) - ( t.z*v.y ) ),
( ( t.z*v.x ) - ( t.x*v.z ) ),
( ( t.x*v.y ) - ( t.y*v.x ) ) );
return cp;
}
//! Project a vector into a plane, defined by its normal
/*! Projects a vector into a plane normal to the given vector, which must
have unit length. Self is modified.
\param v The vector to project
\param n The plane normal
\return The projected vector */
template<class S>
inline const Vector3D<S>& projectNormalTo ( const Vector3D<S>& v, const Vector3D<S> &n) {
S sprod = dot (v, n);
return v - n * dot(v, n);
}
//! Compute the magnitude (length) of the vector
//! (clamps to 0 and 1 with VECTOR_EPSILON)
template<class S>
inline S norm ( const Vector3D<S>& v ) {
S l = v.x*v.x + v.y*v.y + v.z*v.z;
if ( l <= VECTOR_EPSILON*VECTOR_EPSILON ) return(0.);
return ( fabs ( l-1. ) < VECTOR_EPSILON*VECTOR_EPSILON ) ? 1. : sqrt ( l );
}
//! Compute squared magnitude
template<class S>
inline S normSquare ( const Vector3D<S>& v ) {
return v.x*v.x + v.y*v.y + v.z*v.z;
}
//! compatibility, allow use of int, Real and Vec inputs with norm/normSquare
inline Real norm(const Real v) { return fabs(v); }
inline Real normSquare(const Real v) { return square(v); }
inline Real norm(const int v) { return abs(v); }
inline Real normSquare(const int v) { return square(v); }
//! Returns a normalized vector
template<class S>
inline Vector3D<S> getNormalized ( const Vector3D<S>& v ) {
S l = v.x*v.x + v.y*v.y + v.z*v.z;
if ( fabs ( l-1. ) < VECTOR_EPSILON*VECTOR_EPSILON )
return v; /* normalized "enough"... */
else if ( l > VECTOR_EPSILON*VECTOR_EPSILON )
{
S fac = 1./sqrt ( l );
return Vector3D<S> ( v.x*fac, v.y*fac, v.z*fac );
}
else
return Vector3D<S> ( ( S ) 0 );
}
//! Compute the norm of the vector and normalize it.
/*! \return The value of the norm */
template<class S>
inline S normalize ( Vector3D<S> &v ) {
S norm;
S l = v.x*v.x + v.y*v.y + v.z*v.z;
if ( fabs ( l-1. ) < VECTOR_EPSILON*VECTOR_EPSILON ) {
norm = 1.;
} else if ( l > VECTOR_EPSILON*VECTOR_EPSILON ) {
norm = sqrt ( l );
v *= 1./norm;
} else {
v = Vector3D<S>::Zero;
norm = 0.;
}
return ( S ) norm;
}
//! Obtain an orthogonal vector
/*! Compute a vector that is orthonormal to the given vector.
* Nothing else can be assumed for the direction of the new vector.
* \return The orthonormal vector */
template<class S>
Vector3D<S> getOrthogonalVector(const Vector3D<S>& v) {
// Determine the component with max. absolute value
int maxIndex= ( fabs ( v.x ) > fabs ( v.y ) ) ? 0 : 1;
maxIndex= ( fabs ( v[maxIndex] ) > fabs ( v.z ) ) ? maxIndex : 2;
// Choose another axis than the one with max. component and project
// orthogonal to self
Vector3D<S> o ( 0.0 );
o[ ( maxIndex+1 ) %3]= 1;
Vector3D<S> c = cross(v, o);
normalize(c);
return c;
}
//! Convert vector to polar coordinates
/*! Stable vector to angle conversion
*\param v vector to convert
\param phi unique angle [0,2PI]
\param theta unique angle [0,PI]
*/
template<class S>
inline void vecToAngle ( const Vector3D<S>& v, S& phi, S& theta )
{
if ( fabs ( v.y ) < VECTOR_EPSILON )
theta = M_PI/2;
else if ( fabs ( v.x ) < VECTOR_EPSILON && fabs ( v.z ) < VECTOR_EPSILON )
theta = ( v.y>=0 ) ? 0:M_PI;
else
theta = atan ( sqrt ( v.x*v.x+v.z*v.z ) /v.y );
if ( theta<0 ) theta+=M_PI;
if ( fabs ( v.x ) < VECTOR_EPSILON )
phi = M_PI/2;
else
phi = atan ( v.z/v.x );
if ( phi<0 ) phi+=M_PI;
if ( fabs ( v.z ) < VECTOR_EPSILON )
phi = ( v.x>=0 ) ? 0 : M_PI;
else if ( v.z < 0 )
phi += M_PI;
}
//! Compute vector reflected at a surface
/*! Compute a vector, that is self (as an incoming vector)
* reflected at a surface with a distinct normal vector.
* Note that the normal is reversed, if the scalar product with it is positive.
\param t The incoming vector
\param n The surface normal
\return The new reflected vector
*/
template<class S>
inline Vector3D<S> reflectVector ( const Vector3D<S>& t, const Vector3D<S>& n ) {
Vector3D<S> nn= ( dot ( t, n ) > 0.0 ) ? ( n*-1.0 ) : n;
return ( t - nn * ( 2.0 * dot ( nn, t ) ) );
}
//! Compute vector refracted at a surface
/*! \param t The incoming vector
* \param n The surface normal
* \param nt The "inside" refraction index
* \param nair The "outside" refraction index
* \param refRefl Set to 1 on total reflection
* \return The refracted vector
*/
template<class S>
inline Vector3D<S> refractVector ( const Vector3D<S> &t, const Vector3D<S> &normal, S nt, S nair, int &refRefl ) {
// from Glassner's book, section 5.2 (Heckberts method)
S eta = nair / nt;
S n = -dot ( t, normal );
S tt = 1.0 + eta*eta* ( n*n-1.0 );
if ( tt<0.0 ) {
// we have total reflection!
refRefl = 1;
} else {
// normal reflection
tt = eta*n - sqrt ( tt );
return ( t*eta + normal*tt );
}
return t;
}
//! Outputs the object in human readable form as string
template<class S> std::string Vector3D<S>::toString() const {
char buf[256];
snprintf ( buf,256,"[%+4.6f,%+4.6f,%+4.6f]", ( double ) ( *this ) [0], ( double ) ( *this ) [1], ( double ) ( *this ) [2] );
// for debugging, optionally increase precision:
//snprintf ( buf,256,"[%+4.16f,%+4.16f,%+4.16f]", ( double ) ( *this ) [0], ( double ) ( *this ) [1], ( double ) ( *this ) [2] );
return std::string ( buf );
}
template<> std::string Vector3D<int>::toString() const;
//! Outputs the object in human readable form to stream
/*! Output format [x,y,z] */
template<class S>
std::ostream& operator<< ( std::ostream& os, const Vector3D<S>& i ) {
os << i.toString();
return os;
}
//! Reads the contents of the object from a stream
/*! Input format [x,y,z] */
template<class S>
std::istream& operator>> ( std::istream& is, Vector3D<S>& i ) {
char c;
char dummy[3];
is >> c >> i[0] >> dummy >> i[1] >> dummy >> i[2] >> c;
return is;
}
/**************************************************************************/
// Define default vector alias
/**************************************************************************/
//! 3D vector class of type Real (typically float)
typedef Vector3D<Real> Vec3;
//! 3D vector class of type int
typedef Vector3D<int> Vec3i;
//! convert to Real Vector
template<class T> inline Vec3 toVec3 ( T v ) {
return Vec3 ( v[0],v[1],v[2] );
}
//! convert to int Vector
template<class T> inline Vec3i toVec3i ( T v ) {
return Vec3i ( ( int ) v[0], ( int ) v[1], ( int ) v[2] );
}
//! convert to int Vector
template<class T> inline Vec3i toVec3i ( T v0, T v1, T v2 ) {
return Vec3i ( ( int ) v0, ( int ) v1, ( int ) v2 );
}
//! round, and convert to int Vector
template<class T> inline Vec3i toVec3iRound ( T v ) {
return Vec3i ( ( int ) round ( v[0] ), ( int ) round ( v[1] ), ( int ) round ( v[2] ) );
}
//! convert to int Vector if values are close enough to an int
template<class T> inline Vec3i toVec3iChecked ( T v ) {
Vec3i ret;
for (size_t i=0; i<3; i++) {
Real a = v[i];
if (fabs(a-floor(a+0.5)) > 1e-5)
errMsg("argument is not an int, cannot convert");
ret[i] = (int) (a+0.5);
}
return ret;
}
//! convert to double Vector
template<class T> inline Vector3D<double> toVec3d ( T v ) {
return Vector3D<double> ( v[0], v[1], v[2] );
}
//! convert to float Vector
template<class T> inline Vector3D<float> toVec3f ( T v ) {
return Vector3D<float> ( v[0], v[1], v[2] );
}
/**************************************************************************/
// Specializations for common math functions
/**************************************************************************/
template<> inline Vec3 clamp<Vec3>(const Vec3& a, const Vec3& b, const Vec3& c) {
return Vec3 ( clamp(a.x, b.x, c.x),
clamp(a.y, b.y, c.y),
clamp(a.z, b.z, c.z) );
}
template<> inline Vec3 safeDivide<Vec3>(const Vec3 &a, const Vec3& b) {
return Vec3(safeDivide(a.x,b.x), safeDivide(a.y,b.y), safeDivide(a.z,b.z));
}
template<> inline Vec3 nmod<Vec3>(const Vec3& a, const Vec3& b) {
return Vec3(nmod(a.x,b.x),nmod(a.y,b.y),nmod(a.z,b.z));
}
}; // namespace
#endif