#pragma once
#pragma once
#pragma once
#pragma once
#define _USE_MATH_DEFINES
#pragma once
#include<cmath>
#ifndef WIN32
#define __forceinline __attribute__((always_inline))
#endif
namespace heinlib
{
template<typename T>
struct constants
{
static __forceinline T sqrt_2(){return sqrt(T(2));}
static __forceinline T e(){return(T)2.71828182845904523536;}
static __forceinline T pi(){return(T)3.14159265358979323846264338327;}
static __forceinline T two_pi(){return T(2*3.14159265358979323846264338327);}
static __forceinline T sqrt_two_pi(){return T(sqrt(2*3.14159265358979323846264338327));}
static __forceinline T sqrt_half_pi(){return T(sqrt(0.5*3.14159265358979323846264338327));}
static __forceinline T half_pi(){return T(3.14159265358979323846264338327/2);}
static __forceinline T golden_ratio(){return(T)1.6180339887498948482;}
static __forceinline T euler_gamma(){return(T)0.5772156649015328606065120;}
static __forceinline T rad_to_deg(){return 180/constants<T>::pi();}
static __forceinline T deg_to_rad(){return constants<T>::pi()/180;}
};
}
#pragma once
#include<climits>
#include<iterator>
#ifdef _WIN32
#include<cstdint>
#else
#include<stdint.h>
#endif
namespace heinlib
{
template<bool a,bool b>
struct both{
enum{value=0};
};
template<>
struct both<true,true>{
enum{value=1};
};
template<void*A,void*B>struct is_same_ptr{enum{value=0};};
template<void*A>struct is_same_ptr<A,A>{enum{value=1};};
template<unsigned A,unsigned B>struct is_same_value{enum{value=0};};
template<unsigned A>struct is_same_value<A,A>{enum{value=1};};
template<typename T,typename U>struct is_same_type{enum{value=0};};
template<typename T>struct is_same_type<T,T>{enum{value=1};};
template<class A,class B,class C>struct is_same_types{enum{value=0};};
template<class A>struct is_same_types<A,A,A>{enum{value=1};};
template<typename T>struct is_pointer{enum{value=0};};
template<typename T>struct is_pointer<T*>{enum{value=1};};
template<typename T>struct is_const_pointer{enum{value=0};};
template<typename T>struct is_const_pointer<const T*>{enum{value=1};};
template<typename T>struct is_non_const_pointer{enum{value=0};};
template<typename T>struct is_non_const_pointer<const T*>{enum{value=0};};
template<typename T>struct is_non_const_pointer<T*>{enum{value=1};};
namespace detail
{
template<typename T>
long is_iter_helper(typename T::iterator_category*);
template<typename T>
char is_iter_helper(...);
}
template<typename T>
struct is_iter{
enum{value=sizeof(detail::is_iter_helper<T>(0))==sizeof(long)};
};
template<typename T,bool ITER=is_iter<T>::value>
struct is_const_iter{
enum{value=is_const_pointer<typename T::pointer>::value};
};
template<typename T>
struct is_const_iter<T,false>{
enum{value=0};
};
template<typename T,bool ITER=is_iter<T>::value>
struct is_non_const_iter{
enum{value=!is_const_pointer<typename T::pointer>::value};
};
template<typename T>
struct is_non_const_iter<T,false>{
enum{value=0};
};
template<typename T,bool ITER=is_iter<T>::value>
struct is_random_access_iter{
enum{value=(is_same_type<typename T::iterator_category,std::random_access_iterator_tag>::value)};
};
template<typename T>
struct is_random_access_iter<T,false>{
enum{value=0};
};
template<typename T>struct is_basic_type{enum{value=0};};
template<>struct is_basic_type<float>{enum{value=1};};
template<>struct is_basic_type<double>{enum{value=1};};
template<>struct is_basic_type<long double>{enum{value=1};};
template<>struct is_basic_type<bool>{enum{value=1};};
template<>struct is_basic_type<char>{enum{value=1};};
template<>struct is_basic_type<signed char>{enum{value=1};};
template<>struct is_basic_type<signed short>{enum{value=1};};
template<>struct is_basic_type<signed int>{enum{value=1};};
template<>struct is_basic_type<signed long>{enum{value=1};};
template<>struct is_basic_type<signed long long>{enum{value=1};};
template<>struct is_basic_type<unsigned char>{enum{value=1};};
template<>struct is_basic_type<unsigned short>{enum{value=1};};
template<>struct is_basic_type<unsigned int>{enum{value=1};};
template<>struct is_basic_type<unsigned long>{enum{value=1};};
template<>struct is_basic_type<unsigned long long>{enum{value=1};};
template<typename T>struct is_floating{enum{value=0};};
template<>struct is_floating<float>{enum{value=1};};
template<>struct is_floating<double>{enum{value=1};};
template<>struct is_floating<long double>{enum{value=1};};
template<typename T>struct is_integral{enum{value=0};};
template<>struct is_integral<bool>{enum{value=1};};
template<>struct is_integral<char>{enum{value=1};};
template<>struct is_integral<signed char>{enum{value=1};};
template<>struct is_integral<signed short>{enum{value=1};};
template<>struct is_integral<signed int>{enum{value=1};};
template<>struct is_integral<signed long>{enum{value=1};};
template<>struct is_integral<signed long long>{enum{value=1};};
template<>struct is_integral<unsigned char>{enum{value=1};};
template<>struct is_integral<unsigned short>{enum{value=1};};
template<>struct is_integral<unsigned int>{enum{value=1};};
template<>struct is_integral<unsigned long>{enum{value=1};};
template<>struct is_integral<unsigned long long>{enum{value=1};};
template<typename T>struct is_signed{enum{value=0};};
template<>struct is_signed<signed char>{enum{value=1};};
template<>struct is_signed<signed short>{enum{value=1};};
template<>struct is_signed<signed int>{enum{value=1};};
template<>struct is_signed<signed long>{enum{value=1};};
template<>struct is_signed<long long>{enum{value=1};};
template<typename T>struct is_unsigned{enum{value=0};};
template<>struct is_unsigned<bool>{enum{value=1};};
template<>struct is_unsigned<unsigned char>{enum{value=1};};
template<>struct is_unsigned<unsigned short>{enum{value=1};};
template<>struct is_unsigned<unsigned int>{enum{value=1};};
template<>struct is_unsigned<unsigned long>{enum{value=1};};
template<>struct is_unsigned<unsigned long long>{enum{value=1};};
#if CHAR_MAX==SCHAR_MAX
template<>struct is_signed<char>{enum{value=1};};
template<>struct is_unsigned<char>{enum{value=0};};
#else
template<>struct is_signed<char>{enum{value=0};};
template<>struct is_unsigned<char>{enum{value=1};};
#endif
template<typename T>struct get_larger_type{};
template<>struct get_larger_type<float>{typedef double type;};
template<>struct get_larger_type<double>{typedef long double type;};
template<>struct get_larger_type<bool>{typedef uint8_t type;};
template<>struct get_larger_type<uint8_t>{typedef uint16_t type;};
template<>struct get_larger_type<uint16_t>{typedef uint32_t type;};
template<>struct get_larger_type<uint32_t>{typedef uint64_t type;};
template<>struct get_larger_type<char>{typedef int16_t type;};
template<>struct get_larger_type<int8_t>{typedef int16_t type;};
template<>struct get_larger_type<int16_t>{typedef int32_t type;};
template<>struct get_larger_type<int32_t>{typedef int64_t type;};
}
#pragma once
namespace heinlib
{
template<bool B,class T=void>
struct enable_if_c{
typedef T type;
};
template<class T>
struct enable_if_c<false,T>{};
template<class Cond,class T=void>
struct enable_if:public enable_if_c<Cond::value,T>{};
template<bool B,class T=void>
struct disable_if_c{
typedef T type;
};
template<class T>
struct disable_if_c<true,T>{};
template<class Cond,class T=void>
struct disable_if:public disable_if_c<Cond::value,T>{};
}
#pragma once
#include<cmath>
#include<iostream>
#include<cassert>
namespace heinlib
{
#ifdef WIN32
#ifndef _WIN64
__forceinline __declspec(naked)
unsigned __fastcall next_power2(unsigned n){
__asm{
dec ecx
mov eax,1
bsr ecx,ecx
inc ecx
shl eax,cl
ret
}
}
#endif
#else
__forceinline
unsigned next_power2(unsigned n)
{
unsigned result=1;
while(result<n)result<<=1;
return result;
}
#endif
__forceinline bool is_power2(unsigned x)
{
if(x==0)return false;
return((x&(x-1))==0);
}
unsigned power2(unsigned p)
{
assert(p<8*sizeof(unsigned));
unsigned res=1;
res<<=p;
return res;
}
using::pow;
template<typename Int>
typename enable_if<is_integral<Int>,Int>::type
pow(Int base,Int e)
{
Int result=1;
while(e)
{
if(e&1)
result*=base;
e>>=1;
base*=base;
}
return result;
}
template<unsigned N,typename T=float>
struct ipow
{
static T eval(T x)
{
return ipow<N-1,T>::eval(x)*x;
}
};
template<typename T>struct ipow<1,T>
{
static T eval(T x)
{return x;}
};
template<typename T>struct ipow<0,T>
{
static T eval(T x)
{return 1;}
};
#define POW(a,b)POWF((a),(b))
#define POWF(a,b)ipow<(b),float>::eval(a)
#define POWD(a,b)ipow<(b),double>::eval(a)
}
#include<cmath>
#include<algorithm>
#include<limits>
#include<cassert>
namespace heinlib
{
using::sqrt;
using::floor;
using::ceil;
using::log;
template<class T>
T safe_sqrt(T x)
{
if(x<=0)return 0;
return sqrt(x);
}
template<typename T>
typename enable_if<is_integral<T>,bool>::type
even(T x){return(x%2==0);}
template<typename T>
typename enable_if<is_integral<T>,bool>::type
odd(T x){return(x%2!=0);}
template<typename T>
typename enable_if<is_integral<T>,float>::type
sqrt(T x){return sqrt((float)x);}
template<typename T>
typename enable_if<is_integral<T>,float>::type
floor(T x){return floor((float)x);}
template<typename T>
typename enable_if<is_integral<T>,float>::type
ceil(T x){return ceil((float)x);}
template<typename T>
__forceinline T saturate(T x,T minr=0,T maxr=1)
{
if(x<minr)return minr;
if(x>maxr)return maxr;
return x;
}
template<typename T>
__forceinline typename enable_if<is_floating<T>,T>::type
abs(T x)
{
return fabs(x);
}
template<typename T>
__forceinline typename enable_if<is_floating<T>,T>::type
wrap(T x)
{
T discard;
x=modf(x,&discard);
if(x<0)return x+1;
else return x;
}
#ifdef WIN32
template<typename T>
__forceinline typename enable_if<is_basic_type<T>,T>::type
hypot(T a,T b)
{
return(T)_hypot((double)a,(double)b);
}
template<typename T>
__forceinline typename disable_if<is_basic_type<T>,T>::type
hypot(const T&a,const T&b)
{
return sqrt(a*a+b*b);
}
#else
template<typename T>
__forceinline T hypot(T a,T b)
{
return sqrt(a*a+b*b);
}
#endif
template<typename T>
__forceinline T ln(const T&x)
{return log(x);}
template<typename T>
__forceinline T lg(const T&x)
{return log2(x);}
template<typename T>
__forceinline T log(const T&x,const T&base)
{return log(x)/log(base);}
template<typename T>
__forceinline typename disable_if<is_integral<T>,T>::type
log2(const T&x)
{return log(x,T(2));}
template<typename T>
__forceinline typename enable_if<is_integral<T>,T>::type
log2(T x)
{
T r=0;
while(x>>=1)++r;
return r;
}
template<class Real>
__forceinline Real cot(Real x)
{
return tan(constants<Real>::half_pi()-x);
}
template<class Real>
__forceinline Real sec(Real x)
{
return 1/cos(x);
}
template<class Real>
__forceinline Real csc(Real x)
{
return 1/sin(x);
}
template<class Real>
__forceinline Real acot(Real x)
{
if(x<0)return-acot(-x);
return constants<Real>::half_pi()-atan(x);
}
template<typename T>
__forceinline T lerp(T x,T x0,T x1,T y0,T y1)
{
return y0+(x-x0)*(y1-y0)/(x1-x0);
}
template<typename T>
__forceinline T det2(T a,T b,T c,T d)
{
return a*d-b*c;
}
template<typename T>
__forceinline typename enable_if<is_unsigned<T>,T>::type
digits(T x)
{
T d=0;
while(x>0)
{
x/=10;
++d;
}
return d;
}
template<typename T>
__forceinline typename enable_if<is_signed<T>,T>::type
digits(T x)
{
T d=(x<0);
while(x>0)
{
x/=10;
++d;
}
return d;
}
template<typename T>
__forceinline typename enable_if<is_integral<T>,T>::type
idivup(T a,T b){
return(a+b-1)/b;
}
template<typename T>
__forceinline typename enable_if<is_integral<T>,T>::type
mod(T a,T b)
{
return((a>=0)?a%b:a+idivup(-a,b)*b);
}
template<typename T>
__forceinline typename enable_if<is_integral<T>,T>::type
gcd(T a,T b)
{
if(a==b)return a;
return(b==0)?a:gcd(b,mod(a,b));
}
#undef min
#undef max
using std::min;
template<typename T>
T min(const T&a,const T&b,const T&c)
{
return std::min(std::min(a,b),c);
}
using std::max;
template<typename T>
__forceinline T max(const T&a,const T&b,const T&c)
{
return std::max(std::max(a,b),c);
}
template<typename T>
__forceinline const T&min_mag(const T&a,const T&b)
{
return(abs(a)<abs(b)?a:b);
}
template<typename T>
__forceinline const T&min_mag(const T&a,const T&b,const T&c)
{
T abs_a=abs(a),abs_b=abs(b),abs_c=abs(c);
if(abs_a<abs_b)
{
if(abs_a<abs_c)
return a;
return c;
}else if(abs_b<abs_c)
return b;
return c;
}
template<typename T>
__forceinline typename enable_if<is_floating<T>,int>::type
round(T x){
return(int)(x<0?x-T(0.5):x+T(0.5));
};
template<typename T>
__forceinline typename disable_if<is_floating<T>,T>::type
round(T x){return x;};
template<typename T>
__forceinline T relative_difference(T a,T b)
{
return abs(a-b)/std::max(abs(a),abs(b));
}
template<typename T>
__forceinline int sign(T x)
{
if(x==0)return 0;
else return(x>0)?1:-1;
}
template<typename T>
__forceinline int sign2(T x)
{
return(x>=0)?1:-1;
}
template<typename T>__forceinline T sq(T x){return x*x;}
template<typename T>__forceinline T cube(T x){return x*x*x;}
template<typename T>__forceinline bool same_sign(T x,T y)
{
return(fabs(x+y)>=fabs(x-y));
}
template<typename T>__forceinline T sigmoid(T x)
{return T(1/(1+exp(-x)));}
template<typename T>
__forceinline T sigmoid(T x,T sat,T center,T minv,T maxv)
{
return(maxv-minv)/(1+exp(-constants<T>::two_pi()*(x-center)/sat))+minv;
}
template<typename Real>
__forceinline Real gauss(Real x,Real sigma,Real norm)
{
return norm*exp(-Real(0.5f)*sq(x/sigma));
}
template<typename T>
__forceinline T Gaussian2D(T x,T y,T mean_x,T mean_y,T sigma_x,T sigma_y,T amp)
{
return amp*exp(-(sq((x-mean_x)/(2*sigma_x))+sq((y-mean_y)/(2*sigma_y))));
}
template<typename T>
__forceinline T NormalDistrib(T x,T mean,T sigma)
{
T norm=1/(sigma*sqrt(constants<T>::two_pi()));
return Gaussian(x,sigma,norm,mean);
}
template<typename T>
__forceinline typename enable_if<is_floating<T>,bool>::type
not_corrupt(T x)
{
return(x>-std::numeric_limits<T>::infinity()&&
x<std::numeric_limits<T>::infinity());
}
template<typename T>
__forceinline typename disable_if<is_floating<T>,bool>::type
not_corrupt(T x)
{
return true;
}
template<typename T>
__forceinline typename enable_if<is_basic_type<T>,bool>::type
corrupt(T x){return!not_corrupt(x);}
template<typename T>
__forceinline T angle_dist(T a,T b)
{return acos(cos(a)*cos(b)+sin(a)*sin(b));}
template<typename Real>
__forceinline Real angle_dist_flip(Real r1,Real r2)
{
using namespace heinlib;
static const Real H_PI=constants<Real>::half_pi(),PI=constants<Real>::pi();
return fabs(fabs(fmod(r1-r2+H_PI,PI))-H_PI);
}
template<class T>
__forceinline T mid_angle(T a,T b)
{
T half_dist=angle_dist(a,b)/2;
T opt1=a+half_dist;
T opt2=a-half_dist;
if(angle_dist(opt1,b)<angle_dist(opt2,b))
return opt1;
else return opt2;
}
template<typename Real>
__forceinline typename enable_if<is_floating<Real>,Real>::type
std_radian(Real r)
{
static const Real TWO_PI=constants<Real>::two_pi();
if(r<0)return TWO_PI-fmod(-r,TWO_PI);
else return fmod(r,TWO_PI);
}
template<typename Unsigned>
typename enable_if<is_unsigned<Unsigned>,Unsigned>::type
absdiff(Unsigned a,Unsigned b)
{
if(a>=b)return a-b;
else return b-a;
}
template<typename Signed>
typename disable_if<is_unsigned<Signed>,Signed>::type
absdiff(Signed a,Signed b)
{
return abs(a-b);
}
}
#pragma once
#include<iomanip>
#ifndef NO_SSE
#include<xmmintrin.h>
#endif
#include<cstdlib>
#include<cassert>
#include<string>
#include<iostream>
#include<vector>
#pragma once
namespace heinlib
{
template<bool B>
struct heinlib_static_assert{
typedef void STATIC_ASSERTION_FAILED;
};
template<>
struct heinlib_static_assert<false>{};
}
#define STATIC_ASSERT(Cond,ErrorMsg)typedef typename heinlib::heinlib_static_assert<(Cond)>::STATIC_ASSERTION_FAILED ErrorMsg
#pragma once
#pragma once
#ifdef WIN32
#define DEP_INEF __declspec(deprecated(INEFFICIENT_CALL))
#else
#define DEP_INEF __attribute__((deprecated))
#define __forceinline __attribute__((always_inline))
#endif
namespace heinlib
{
template<typename Real>
struct dynamic_matrix;
template<unsigned M,unsigned N,typename Real>
struct static_matrix;
template<typename T>struct is_matrix{enum{value=0};};
template<unsigned M,unsigned N,typename Real>
struct is_matrix<static_matrix<M,N,Real> >{enum{value=1};};
template<typename Real>
struct is_matrix<dynamic_matrix<Real> >{enum{value=1};};
template<typename Matrix>
__forceinline typename enable_if<is_matrix<Matrix>,bool>::type
is_square(const Matrix&in)
{
return(in.rows==in.cols);
}
template<typename Matrix>
__forceinline typename disable_if<is_matrix<Matrix>,bool>::type
is_square(const Matrix&in)
{
return false;
}
template<typename Matrix>
__forceinline typename enable_if<is_matrix<Matrix>,bool>::type
size_is(const Matrix&in,unsigned M,unsigned N)
{
return(in.rows==M&&in.cols==N);
}
template<typename MI,typename MI2>
__forceinline typename enable_if_c<is_matrix<MI>::value&&is_matrix<MI2>::value,bool>::type
same_dim(const MI&in,const MI2&in2)
{
return(in.rows==in2.rows&&in.cols==in2.cols);
}
}
#pragma once
#include<istream>
#include<string>
#ifdef WIN32
#define __GXX_EXPERIMENTAL_CXX0X__
#endif
namespace heinlib
{
inline std::istream&getline(
#ifdef __GXX_EXPERIMENTAL_CXX0X__
std::istream&&_Istr,
#else
std::istream&_Istr,
#endif
std::string&_Str,
std::string _Delim)
{
using namespace std;
typedef std::istream _Myis;
typedef std::istream::traits_type _Traits;
ios_base::iostate _State=ios_base::goodbit;
bool _Changed=false;
const _Myis::sentry _Ok(_Istr,true);
if(_Ok)
{
try
{
_Str.erase();
_Istr.rdbuf()->sgetc();
_Traits::int_type _Meta=_Istr.rdbuf()->sgetc();
for(;;_Meta=_Istr.rdbuf()->snextc())
if(_Traits::eq_int_type(_Traits::eof(),_Meta))
{
_State|=ios_base::eofbit;
break;
}
else if(_Delim.find(_Meta)!=string::npos)
{
_Changed=true;
_Istr.rdbuf()->sbumpc();
break;
}
else if(_Str.max_size()<=_Str.size())
{
_State|=ios_base::failbit;
break;
}
else
{
_Str+=_Traits::to_char_type(_Meta);
_Changed=true;
}
}catch(...){
_Istr.setstate(ios_base::badbit);
}
}
if(!_Changed)
_State|=ios_base::failbit;
_Istr.setstate(_State);
return(_Istr);
}
#ifdef __GXX_EXPERIMENTAL_CXX0X__
inline std::istream&getline(
std::istream&_Istr,
std::string&_Str,
std::string _Delim)
{
return heinlib::getline(std::move(_Istr),_Str,_Delim);
}
#endif
}
#pragma once
#include<sstream>
#include<cstdlib>
#include<stdexcept>
#include<typeinfo>
namespace heinlib
{
template<class T>
__forceinline bool convert(T&out,const std::string&in)
{
std::istringstream iss(in);
iss>>out;
return(iss.eof()&&!iss.fail());
}
template<>
__forceinline bool convert<std::string>(std::string&out,const std::string&in)
{
out=in;
return true;
}
template<class T>
__forceinline T convert(const std::string&in)
{
T out;
if(!convert(out,in))throw std::runtime_error("Cannot convert '"+in+"' to "+typeid(out).name());
return out;
}
template<class T>
__forceinline T convert(const std::string&in,const T default_value)
{
T out;
return convert(out,in)?out:default_value;
}
std::string BytesToHex(char*data,unsigned size)
{
using namespace std;
stringstream ss;
for(unsigned i=0;i<size;++i)
ss<<hex<<uppercase<<unsigned(data[i]);
return ss.str();
}
}
namespace heinlib
{
#pragma region FORWARD_DECLARATIONS
template<typename Real>
struct dynamic_matrix;
template<unsigned M,unsigned N,typename Real>
struct static_matrix;
typedef static_matrix<2,2,float>matrix_2x2f;
typedef static_matrix<2,3,float>matrix_2x3f;
typedef static_matrix<3,3,float>matrix_3x3f;
typedef static_matrix<3,4,float>matrix_3x4f;
typedef static_matrix<4,4,float>matrix_4x4f;
typedef static_matrix<2,2,double>matrix_2x2d;
typedef static_matrix<3,3,double>matrix_3x3d;
typedef static_matrix<3,4,double>matrix_3x4d;
typedef static_matrix<4,4,double>matrix_4x4d;
typedef static_matrix<2,1,unsigned>vector_2u;
typedef static_matrix<3,1,unsigned>vector_3u;
typedef static_matrix<4,1,unsigned>vector_4u;
typedef static_matrix<2,1,int>vector_2i;
typedef static_matrix<3,1,int>vector_3i;
typedef static_matrix<4,1,int>vector_4i;
typedef static_matrix<2,1,float>vector_2f;
typedef static_matrix<3,1,float>vector_3f;
typedef static_matrix<4,1,float>vector_4f;
typedef static_matrix<5,1,float>vector_5f;
typedef static_matrix<6,1,float>vector_6f;
typedef static_matrix<2,1,double>vector_2d;
typedef static_matrix<3,1,double>vector_3d;
typedef static_matrix<4,1,double>vector_4d;
#pragma endregion
#pragma region CLASS_DEFS
template<unsigned M,unsigned N,typename Real>
struct static_matrix
{
static const unsigned rows=M,cols=N;
typedef Real real_type;
Real data[M*N];
Real*ptr()
{
return&data[0];
}
const Real*ptr()const
{
return&data[0];
}
__forceinline unsigned size()const{return M*N;}
__forceinline const Real&x()const{return data[0];}
__forceinline Real&x(){return data[0];}
__forceinline const Real&y()const{return data[1];}
__forceinline Real&y(){return data[1];}
__forceinline const Real&z()const{return data[2];}
__forceinline Real&z(){return data[2];}
__forceinline const Real&w()const{return data[3];}
__forceinline Real&w(){return data[3];}
__forceinline const Real&operator[](unsigned i)const
{
assert(i<M*N);
return data[i];
}
__forceinline Real&operator[](unsigned i)
{
assert(i<M*N);
return data[i];
}
__forceinline const Real&operator()(unsigned row,unsigned col)const
{
assert(row<M);
assert(col<N);
return data[row*N+col];
}
__forceinline Real&operator()(unsigned row,unsigned col)
{
assert(row<M);
assert(col<N);
return data[row*N+col];
}
template<class Real2>
__forceinline
typename enable_if<is_basic_type<Real2>,static_matrix&>::type
operator=(const Real2&val)
{
for(unsigned i=0;i<M*N;++i)
data[i]=(Real)val;
return*this;
}
template<typename Real2>
__forceinline static_matrix&operator=(const dynamic_matrix<Real2>&in)
{
assert(rows*cols==in.rows*in.cols);
for(unsigned i=0;i<M*N;++i)
data[i]=(Real)in[i];
return*this;
}
template<typename Real2>
__forceinline static_matrix&operator=(const static_matrix<M,N,Real2>&in)
{
assert(rows*cols==in.rows*in.cols);
for(unsigned i=0;i<M*N;++i)
data[i]=(Real)in.data[i];
return*this;
}
__forceinline bool operator==(const static_matrix&in)const
{
for(unsigned i=0;i<M*N;++i)
if(data[i]!=in[i])return false;
return true;
}
__forceinline bool operator!=(const static_matrix&in)const
{
for(unsigned i=0;i<M*N;++i)
if(data[i]!=in[i])return true;
return false;
}
__forceinline static_matrix<1,N,Real>get_row(unsigned m)const
{
static_matrix<1,N,Real>ret;
for(unsigned n=0;n<N;++n)
ret[n]=(*this)(m,n);
return ret;
}
__forceinline static_matrix<M,1,Real>get_column(unsigned n)const
{
static_matrix<M,1,Real>ret;
for(unsigned m=0;m<M;++m)
ret[m]=(*this)(m,n);
return ret;
}
template<unsigned M1,unsigned N1,unsigned M2,unsigned N2>
__forceinline static_matrix<M2-M1+1,N2-N1+1,Real>submatrix()const
{
assert(M2>=M1&&N2>=N1);
assert(M2<M&&N2<N);
static_matrix<M2-M1+1,N2-N1+1,Real>ret;
for(unsigned m=M1;m<=M2;++m)
for(unsigned n=N1;n<=N2;++n)
ret(m-M1,n-N1)=(*this)(m,n);
return ret;
}
__forceinline dynamic_matrix<Real>submatrix(unsigned M1,unsigned N1,unsigned M2,unsigned N2)const
{
assert(M2>=M1&&N2>=N1);
assert(M2<M&&N2<N);
dynamic_matrix<Real>ret(M2-M1+1,N2-N1+1);
for(unsigned m=M1;m<=M2;++m)
for(unsigned n=N1;n<=N2;++n)
ret(m-M1,n-N1)=(*this)(m,n);
return ret;
}
template<typename MI>
__forceinline void block_insert(unsigned M1,unsigned N1,const MI&in)
{
assert(M1+in.rows<=rows);
assert(N1+in.cols<=cols);
for(unsigned r=0;r<in.rows;++r)
for(unsigned c=0;c<in.cols;++c)
(*this)(r+M1,c+N1)=in(r,c);
}
template<typename MI>
__forceinline void block_insert_add(unsigned M1,unsigned N1,const MI&in)
{
assert(M1+in.rows<=rows);
assert(N1+in.cols<=cols);
for(unsigned r=0;r<in.rows;++r)
for(unsigned c=0;c<in.cols;++c)
(*this)(r+M1,c+N1)+=in(r,c);
}
template<typename MI>
__forceinline void block_insert(unsigned M1,unsigned N1,unsigned M2,unsigned N2,const MI&in)
{
assert(M1+in.rows-M2<=rows);
assert(N1+in.cols-N2<=cols);
for(unsigned r=M2;r<in.rows;++r)
for(unsigned c=N2;c<in.cols;++c)
(*this)(r-M2+M1,c-N2+N1)=in(r,c);
}
template<unsigned P>
__forceinline static_matrix<M,P,Real>operator*(const static_matrix<N,P,Real>&m)const;
__forceinline static_matrix<M,N,Real>operator+(const static_matrix<M,N,Real>&m)const;
__forceinline static_matrix<M,N,Real>operator-(const static_matrix<M,N,Real>&m)const;
__forceinline static_matrix<M,N,Real>&operator*=(const static_matrix<M,N,Real>&m);
__forceinline static_matrix<M,N,Real>&operator+=(const static_matrix<M,N,Real>&m);
__forceinline static_matrix<M,N,Real>&operator-=(const static_matrix<M,N,Real>&m);
__forceinline static_matrix<M,N,Real>operator%(const static_matrix<M,N,Real>&m)const;
__forceinline dynamic_matrix<Real>operator%(const dynamic_matrix<Real>&m)const;
__forceinline dynamic_matrix<Real>operator*(const dynamic_matrix<Real>&m)const;
__forceinline static_matrix<M,N,Real>operator+(const dynamic_matrix<Real>&m)const;
__forceinline static_matrix<M,N,Real>operator-(const dynamic_matrix<Real>&m)const;
__forceinline static_matrix<M,N,Real>&operator+=(const dynamic_matrix<Real>&m);
__forceinline static_matrix<M,N,Real>&operator-=(const dynamic_matrix<Real>&m);
__forceinline static_matrix<M,N,Real>operator+(Real s)const;
__forceinline static_matrix<M,N,Real>operator-(Real s)const;
__forceinline static_matrix<M,N,Real>operator*(Real s)const;
__forceinline static_matrix<M,N,Real>operator/(Real s)const;
__forceinline static_matrix<M,N,Real>&operator+=(Real s);
__forceinline static_matrix<M,N,Real>&operator-=(Real s);
__forceinline static_matrix<M,N,Real>&operator*=(Real s);
__forceinline static_matrix<M,N,Real>&operator/=(Real s);
};
template<typename Real>
struct dynamic_matrix
{
unsigned rows,cols;
typedef Real real_type;
std::vector<Real>data;
__forceinline unsigned size()const{return rows*cols;}
__forceinline dynamic_matrix():rows(0),cols(0){}
__forceinline dynamic_matrix(unsigned M)
:rows(M),cols(1),data(M){}
__forceinline dynamic_matrix(unsigned M,unsigned N)
:rows(M),cols(N),data(M*N){}
__forceinline dynamic_matrix(const dynamic_matrix<Real>&in)
:rows(in.rows),cols(in.cols),data(in.rows*in.cols)
{
std::copy(in.data.begin(),in.data.end(),data.begin());
}
__forceinline dynamic_matrix(const std::vector<Real>&in)
:rows(in.size()),cols(1),data(in.size())
{
std::copy(in.begin(),in.end(),data.begin());
}
template<unsigned M,unsigned N>
__forceinline dynamic_matrix(const static_matrix<M,N,Real>&in)
:rows(in.rows),cols(in.cols),data(in.rows*in.cols)
{
for(unsigned i=0;i<in.rows*in.cols;++i)
data[i]=in[i];
}
__forceinline void resize(unsigned M,unsigned N)
{
data.resize(M*N);
rows=M;
cols=N;
}
__forceinline void operator=(const dynamic_matrix<Real>&in)
{
resize(in.rows,in.cols);
std::copy(in.data.begin(),in.data.end(),data.begin());
}
template<unsigned M,unsigned N>
__forceinline void operator=(const static_matrix<M,N,Real>&in)
{
resize(in.rows,in.cols);
for(unsigned i=0;i<in.rows*in.cols;++i)
data[i]=in[i];
}
template<typename T>
__forceinline void operator=(const dynamic_matrix<T>&in)
{
resize(in.rows,in.cols);
for(unsigned i=0;i<in.rows*in.cols;++i)
data[i]=(Real)in[i];
}
__forceinline const Real&operator[](unsigned i)const
{
assert(i<rows*cols);
return data[i];
}
__forceinline Real&operator[](unsigned i)
{
assert(i<rows*cols);
return data[i];
}
__forceinline const Real&operator()(unsigned row,unsigned col)const
{
assert(row<rows);
assert(col<cols);
return data[row*cols+col];
}
__forceinline Real&operator()(unsigned row,unsigned col)
{
assert(row<rows);
assert(col<cols);
return data[row*cols+col];
}
__forceinline bool operator==(const dynamic_matrix&in)const
{
if(rows!=in.rows||cols!=in.cols)
return false;
for(unsigned i=0;i<rows*cols;++i)
if(data[i]!=in[i])return false;
return true;
}
__forceinline bool operator!=(const dynamic_matrix&in)const
{
if(rows!=in.rows||cols!=in.cols)
return true;
for(unsigned i=0;i<rows*cols;++i)
if(data[i]!=in[i])return true;
return false;
}
__forceinline dynamic_matrix<Real>get_row(unsigned m)const
{
dynamic_matrix<Real>ret(1,cols);
for(unsigned n=0;n<cols;++n)
ret[n]=(*this)(m,n);
return ret;
}
__forceinline dynamic_matrix<Real>get_column(unsigned n)const
{
dynamic_matrix<Real>ret(rows,1);
for(unsigned m=0;m<rows;++m)
ret[m]=(*this)(m,n);
return ret;
}
__forceinline dynamic_matrix<Real>submatrix(unsigned M1,unsigned N1,unsigned M2,unsigned N2)const
{
assert(M2>=M1&&N2>=N1);
assert(M2<rows&&N2<cols);
dynamic_matrix<Real>ret(M2-M1+1,N2-N1+1);
for(unsigned m=M1;m<=M2;++m)
for(unsigned n=N1;n<=N2;++n)
ret(m-M1,n-N1)=(*this)(m,n);
return ret;
}
template<typename MI>
__forceinline void block_insert(unsigned M1,unsigned N1,const MI&in)
{
assert(M1+in.rows<=rows);
assert(N1+in.cols<=cols);
for(unsigned r=0;r<in.rows;++r)
for(unsigned c=0;c<in.cols;++c)
(*this)(r+M1,c+N1)=in(r,c);
}
template<typename MI>
__forceinline void block_insert_add(unsigned M1,unsigned N1,const MI&in)
{
assert(M1+in.rows<=rows);
assert(N1+in.cols<=cols);
for(unsigned r=0;r<in.rows;++r)
for(unsigned c=0;c<in.cols;++c)
(*this)(r+M1,c+N1)+=in(r,c);
}
template<typename MI>
__forceinline void block_insert(unsigned M1,unsigned N1,unsigned M2,unsigned N2,const MI&in)
{
assert(M1+in.rows-M2<=rows);
assert(N1+in.cols-N2<=cols);
for(unsigned r=M2;r<in.rows;++r)
for(unsigned c=N2;c<in.cols;++c)
(*this)(r-M2+M1,c-N2+N1)=in(r,c);
}
__forceinline dynamic_matrix<Real>operator*(const dynamic_matrix<Real>&m)const;
__forceinline dynamic_matrix<Real>operator+(const dynamic_matrix<Real>&m)const;
__forceinline dynamic_matrix<Real>operator-(const dynamic_matrix<Real>&m)const;
__forceinline dynamic_matrix<Real>&operator+=(const dynamic_matrix<Real>&m);
__forceinline dynamic_matrix<Real>&operator-=(const dynamic_matrix<Real>&m);
__forceinline dynamic_matrix<Real>&operator*=(const dynamic_matrix<Real>&m);
template<unsigned M,unsigned N>
__forceinline dynamic_matrix<Real>&operator*=(const static_matrix<M,N,Real>&m);
template<unsigned M,unsigned N>
__forceinline dynamic_matrix<Real>operator*(const static_matrix<M,N,Real>&m)const;
template<unsigned M,unsigned N>
__forceinline static_matrix<M,N,Real>operator+(const static_matrix<M,N,Real>&m)const;
template<unsigned M,unsigned N>
__forceinline static_matrix<M,N,Real>operator-(const static_matrix<M,N,Real>&m)const;
template<unsigned M,unsigned N>
__forceinline dynamic_matrix<Real>&operator+=(const static_matrix<M,N,Real>&m);
template<unsigned M,unsigned N>
__forceinline dynamic_matrix<Real>&operator-=(const static_matrix<M,N,Real>&m);
__forceinline dynamic_matrix<Real>operator+(Real s)const;
__forceinline dynamic_matrix<Real>operator-(Real s)const;
__forceinline dynamic_matrix<Real>operator*(Real s)const;
__forceinline dynamic_matrix<Real>operator/(Real s)const;
__forceinline dynamic_matrix<Real>&operator+=(Real s);
__forceinline dynamic_matrix<Real>&operator-=(Real s);
__forceinline dynamic_matrix<Real>&operator*=(Real s);
__forceinline dynamic_matrix<Real>&operator/=(Real s);
};
#pragma endregion
#pragma region BASIC_MEMFUNCS_STATIC
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>operator-(const static_matrix<M,N,Real>&m)
{return m*Real(-1);}
template<unsigned M,unsigned N,typename Real,typename T>
__forceinline typename enable_if_c<is_basic_type<T>::value||is_same_type<T,Real>::value,static_matrix<M,N,Real> >::type
operator*(T s,const static_matrix<M,N,Real>&m)
{return m*Real(s);}
template<unsigned M,unsigned N,typename Real,typename T>
__forceinline typename enable_if_c<is_basic_type<T>::value||is_same_type<T,Real>::value,static_matrix<M,N,Real> >::type
operator/(T s,const static_matrix<M,N,Real>&m)
{
Real ss=(Real)s;
static_matrix<M,N,Real>ret;
for(unsigned i=0;i<M*N;++i)
ret[i]=ss/m[i];
return ret;
}
template<unsigned M,unsigned N,typename Real,typename T>
__forceinline typename enable_if_c<is_basic_type<T>::value||is_same_type<T,Real>::value,static_matrix<M,N,Real> >::type
operator+(T s,const static_matrix<M,N,Real>&m)
{return m+Real(s);}
template<unsigned M,unsigned N,typename Real>
template<unsigned P>
__forceinline static_matrix<M,P,Real>static_matrix<M,N,Real>::operator*(const static_matrix<N,P,Real>&m)const
{
static_matrix<M,P,Real>ret;
for(unsigned i=0;i<M;++i){
for(unsigned j=0;j<P;++j){
Real sum=0;
for(unsigned k=0;k<N;++k)
sum+=(*this)(i,k)*m(k,j);
ret(i,j)=sum;
}
}
return ret;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>&static_matrix<M,N,Real>::operator*=(const static_matrix<M,N,Real>&m)
{
STATIC_ASSERT(M==N,TIMES_EQUAL_OP_ONLY_FOR_SQUARE_MATRICES);
*this=*this*m;
return*this;
}
template<unsigned M,unsigned N,typename Real>
__forceinline dynamic_matrix<Real>static_matrix<M,N,Real>::operator*(const dynamic_matrix<Real>&m)const
{
dynamic_matrix<Real>ret;
assert(cols==m.rows);
ret.resize(rows,m.cols);
for(unsigned i=0;i<rows;++i){
for(unsigned j=0;j<m.cols;++j){
Real sum=0;
for(unsigned k=0;k<cols;++k)
sum+=(*this)(i,k)*m(k,j);
ret(i,j)=sum;
}
}
return ret;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>static_matrix<M,N,Real>::operator%(const static_matrix<M,N,Real>&m)const
{
static_matrix<M,N,Real>ret;
for(unsigned i=0;i<M*N;++i)
ret[i]=(*this)[i]*m[i];
return ret;
}
template<unsigned M,unsigned N,typename Real>
__forceinline dynamic_matrix<Real>static_matrix<M,N,Real>::operator%(const dynamic_matrix<Real>&m)const
{
dynamic_matrix<Real>ret(m.rows,m.cols);
for(unsigned i=0;i<M*N;++i)
ret[i]=(*this)[i]*m[i];
return ret;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>static_matrix<M,N,Real>::operator+(const static_matrix<M,N,Real>&m)const
{
static_matrix<M,N,Real>ret;
for(unsigned i=0;i<M;++i)
for(unsigned j=0;j<N;++j)
ret(i,j)=(*this)(i,j)+m(i,j);
return ret;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>static_matrix<M,N,Real>::operator-(const static_matrix<M,N,Real>&m)const
{
static_matrix<M,N,Real>ret;
for(unsigned i=0;i<M;++i)
for(unsigned j=0;j<N;++j)
ret(i,j)=(*this)(i,j)-m(i,j);
return ret;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>&static_matrix<M,N,Real>::operator+=(const static_matrix<M,N,Real>&m)
{
for(unsigned i=0;i<M;++i)
for(unsigned j=0;j<N;++j)
(*this)(i,j)+=m(i,j);
return*this;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>&static_matrix<M,N,Real>::operator-=(const static_matrix<M,N,Real>&m)
{
for(unsigned i=0;i<M;++i)
for(unsigned j=0;j<N;++j)
(*this)(i,j)-=m(i,j);
return*this;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>static_matrix<M,N,Real>::operator+(const dynamic_matrix<Real>&m)const
{
assert(same_dim(*this,m));
static_matrix<M,N,Real>ret;
for(unsigned i=0;i<M;++i)
for(unsigned j=0;j<N;++j)
ret(i,j)=(*this)(i,j)+m(i,j);
return ret;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>static_matrix<M,N,Real>::operator-(const dynamic_matrix<Real>&m)const
{
assert(same_dim(*this,m));
static_matrix<M,N,Real>ret;
for(unsigned i=0;i<M;++i)
for(unsigned j=0;j<N;++j)
ret(i,j)=(*this)(i,j)-m(i,j);
return ret;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>&static_matrix<M,N,Real>::operator+=(const dynamic_matrix<Real>&m)
{
assert(same_dim(*this,m));
for(unsigned i=0;i<M;++i)
for(unsigned j=0;j<N;++j)
(*this)(i,j)+=m(i,j);
return*this;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>&static_matrix<M,N,Real>::operator-=(const dynamic_matrix<Real>&m)
{
assert(same_dim(*this,m));
for(unsigned i=0;i<M;++i)
for(unsigned j=0;j<N;++j)
(*this)(i,j)-=m(i,j);
return*this;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>static_matrix<M,N,Real>::operator+(Real s)const
{
static_matrix<M,N,Real>ret;
for(unsigned i=0;i<M*N;++i)
ret[i]=(*this)[i]+s;
return ret;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>static_matrix<M,N,Real>::operator-(Real s)const
{
static_matrix<M,N,Real>ret;
for(unsigned i=0;i<M*N;++i)
ret[i]=(*this)[i]-s;
return ret;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>static_matrix<M,N,Real>::operator*(Real s)const
{
static_matrix<M,N,Real>ret;
for(unsigned i=0;i<M*N;++i)
ret[i]=(*this)[i]*s;
return ret;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>static_matrix<M,N,Real>::operator/(Real s)const
{
static_matrix<M,N,Real>ret;
for(unsigned i=0;i<M*N;++i)
ret[i]=(*this)[i]/s;
return ret;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>&static_matrix<M,N,Real>::operator+=(Real s)
{
static_matrix<M,N,Real>ret;
for(unsigned i=0;i<M*N;++i)
(*this)[i]+=s;
return ret;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>&static_matrix<M,N,Real>::operator-=(Real s)
{
static_matrix<M,N,Real>ret;
for(unsigned i=0;i<M*N;++i)
(*this)[i]-=s;
return ret;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>&static_matrix<M,N,Real>::operator*=(Real s)
{
for(unsigned i=0;i<M*N;++i)
(*this)[i]*=s;
return*this;
}
template<unsigned M,unsigned N,typename Real>
__forceinline static_matrix<M,N,Real>&static_matrix<M,N,Real>::operator/=(Real s)
{
for(unsigned i=0;i<M*N;++i)
(*this)[i]/=s;
return*this;
}
#pragma endregion
#pragma region BASIC_MEMFUNCS_DYNAMIC
template<typename Real>
__forceinline dynamic_matrix<Real>operator-(const dynamic_matrix<Real>&m)
{return m*Real(-1);}
template<typename Real,typename T>
__forceinline typename enable_if_c<is_basic_type<T>::value||is_same_type<T,Real>::value,dynamic_matrix<Real> >::type
operator*(T s,const dynamic_matrix<Real>&m)
{return m*Real(s);}
template<typename Real,typename T>
__forceinline typename enable_if_c<is_basic_type<T>::value||is_same_type<T,Real>::value,dynamic_matrix<Real> >::type
operator/(T s,const dynamic_matrix<Real>&m)
{
Real ss=(Real)s;
dynamic_matrix<Real>ret;
for(unsigned i=0;i<m.rows*m.cols;++i)
ret[i]=ss/m[i];
return ret;
}
template<typename Real,typename T>
__forceinline typename enable_if_c<is_basic_type<T>::value||is_same_type<T,Real>::value,dynamic_matrix<Real> >::type
operator+(T s,const dynamic_matrix<Real>&m)
{
return m+Real(s);
}
template<typename Real>
__forceinline dynamic_matrix<Real>dynamic_matrix<Real>::operator*(const dynamic_matrix<Real>&m)const
{
dynamic_matrix<Real>ret(rows,m.cols);
assert(cols==m.rows);
for(unsigned i=0;i<ret.rows;++i){
for(unsigned j=0;j<ret.cols;++j){
Real sum=0;
for(unsigned k=0;k<m.rows;++k)
sum+=(*this)(i,k)*m(k,j);
ret(i,j)=sum;
}
}
return ret;
}
template<typename Real>
__forceinline dynamic_matrix<Real>dynamic_matrix<Real>::operator+(const dynamic_matrix<Real>&m)const
{
assert(same_dim(*this,m));
dynamic_matrix<Real>ret(rows,cols);
for(unsigned i=0;i<rows;++i)
for(unsigned j=0;j<cols;++j)
ret(i,j)=(*this)(i,j)+m(i,j);
return ret;
}
template<typename Real>
__forceinline dynamic_matrix<Real>dynamic_matrix<Real>::operator-(const dynamic_matrix<Real>&m)const
{
assert(same_dim(*this,m));
dynamic_matrix<Real>ret(rows,cols);
for(unsigned i=0;i<rows;++i)
for(unsigned j=0;j<cols;++j)
ret(i,j)=(*this)(i,j)-m(i,j);
return ret;
}
template<typename Real>
__forceinline dynamic_matrix<Real>&dynamic_matrix<Real>::operator+=(const dynamic_matrix<Real>&m)
{
assert(same_dim(*this,m));
for(unsigned i=0;i<rows;++i)
for(unsigned j=0;j<cols;++j)
(*this)(i,j)+=m(i,j);
return*this;
}
template<typename Real>
__forceinline dynamic_matrix<Real>&dynamic_matrix<Real>::operator-=(const dynamic_matrix<Real>&m)
{
assert(same_dim(*this,m));
for(unsigned i=0;i<rows;++i)
for(unsigned j=0;j<cols;++j)
(*this)(i,j)-=m(i,j);
return*this;
}
template<typename Real>
template<unsigned M,unsigned N>
__forceinline dynamic_matrix<Real>dynamic_matrix<Real>::operator*(const static_matrix<M,N,Real>&m)const
{
dynamic_matrix<Real>ret(rows,m.cols);
assert(cols==m.rows);
for(unsigned i=0;i<ret.rows;++i){
for(unsigned j=0;j<ret.cols;++j){
Real sum=0;
for(unsigned k=0;k<m.rows;++k)
sum+=(*this)(i,k)*m(k,j);
ret(i,j)=sum;
}
}
return ret;
}
template<typename Real>
template<unsigned M,unsigned N>
__forceinline static_matrix<M,N,Real>dynamic_matrix<Real>::operator+(const static_matrix<M,N,Real>&m)const
{
assert(same_dim(*this,m));
static_matrix<M,N,Real>ret;
for(unsigned i=0;i<M;++i)
for(unsigned j=0;j<N;++j)
ret(i,j)=(*this)(i,j)+m(i,j);
return ret;
}
template<typename Real>
template<unsigned M,unsigned N>
__forceinline static_matrix<M,N,Real>dynamic_matrix<Real>::operator-(const static_matrix<M,N,Real>&m)const
{
assert(same_dim(*this,m));
static_matrix<M,N,Real>ret;
for(unsigned i=0;i<M;++i)
for(unsigned j=0;j<N;++j)
ret(i,j)=(*this)(i,j)-m(i,j);
return ret;
}
template<typename Real>
template<unsigned M,unsigned N>
__forceinline dynamic_matrix<Real>&dynamic_matrix<Real>::operator+=(const static_matrix<M,N,Real>&m)
{
assert(same_dim(*this,m));
for(unsigned i=0;i<rows;++i)
for(unsigned j=0;j<cols;++j)
(*this)(i,j)+=m(i,j);
return*this;
}
template<typename Real>
template<unsigned M,unsigned N>
__forceinline dynamic_matrix<Real>&dynamic_matrix<Real>::operator-=(const static_matrix<M,N,Real>&m)
{
assert(same_dim(*this,m));
for(unsigned i=0;i<rows;++i)
for(unsigned j=0;j<cols;++j)
(*this)(i,j)-=m(i,j);
return*this;
}
template<typename Real>
__forceinline dynamic_matrix<Real>dynamic_matrix<Real>::operator+(Real s)const
{
dynamic_matrix<Real>ret(rows,cols);
for(unsigned i=0;i<rows;++i)
for(unsigned j=0;j<cols;++j)
ret(i,j)=(*this)(i,j)+s;
return ret;
}
template<typename Real>
__forceinline dynamic_matrix<Real>dynamic_matrix<Real>::operator-(Real s)const
{
dynamic_matrix<Real>ret(rows,cols);
for(unsigned i=0;i<rows;++i)
for(unsigned j=0;j<cols;++j)
ret(i,j)=(*this)(i,j)-s;
return ret;
}
template<typename Real>
__forceinline dynamic_matrix<Real>dynamic_matrix<Real>::operator*(Real s)const
{
dynamic_matrix<Real>ret(rows,cols);
for(unsigned i=0;i<rows;++i)
for(unsigned j=0;j<cols;++j)
ret(i,j)=(*this)(i,j)*s;
return ret;
}
template<typename Real>
__forceinline dynamic_matrix<Real>dynamic_matrix<Real>::operator/(Real s)const
{
dynamic_matrix<Real>ret(rows,cols);
for(unsigned i=0;i<rows;++i)
for(unsigned j=0;j<cols;++j)
ret(i,j)=(*this)(i,j)/s;
return ret;
}
template<typename Real>
__forceinline dynamic_matrix<Real>&dynamic_matrix<Real>::operator+=(Real s)
{
for(unsigned i=0;i<rows;++i)
for(unsigned j=0;j<cols;++j)
(*this)(i,j)+=s;
return*this;
}
template<typename Real>
__forceinline dynamic_matrix<Real>&dynamic_matrix<Real>::operator-=(Real s)
{
for(unsigned i=0;i<rows;++i)
for(unsigned j=0;j<cols;++j)
(*this)(i,j)-=s;
return*this;
}
template<typename Real>
__forceinline dynamic_matrix<Real>&dynamic_matrix<Real>::operator*=(Real s)
{
for(unsigned i=0;i<rows;++i)
for(unsigned j=0;j<cols;++j)
(*this)(i,j)*=s;
return*this;
}
template<typename Real>
__forceinline dynamic_matrix<Real>&dynamic_matrix<Real>::operator/=(Real s)
{
for(unsigned i=0;i<rows;++i)
for(unsigned j=0;j<cols;++j)
(*this)(i,j)/=s;
return*this;
}
template<typename Real>
__forceinline dynamic_matrix<Real>&dynamic_matrix<Real>::operator*=(const dynamic_matrix<Real>&m)
{
*this=*this*m;
return*this;
}
template<typename Real>
template<unsigned M,unsigned N>
__forceinline dynamic_matrix<Real>&dynamic_matrix<Real>::operator*=(const static_matrix<M,N,Real>&m)
{
*this=*this*m;
return*this;
}
#pragma endregion
#pragma region OSTREAM_STUFF
template<class Matrix>
__forceinline typename enable_if<is_matrix<Matrix>,std::ostream&>::type
operator<<(std::ostream&os,const Matrix&m)
{
std::streamsize width=os.width();
if(m.cols==1)
{
os<<"(";
unsigned i=0;
while(i+1<m.rows){
os<<std::setw(width)<<m[i]<<", ";
++i;
}
if(m.rows>0)
os<<std::setw(width)<<m[i];
os<<")";
return os;
}else
{
os<<"[ ";
for(unsigned i=0;i+1<m.rows;++i)
{
for(unsigned j=0;j+1<m.cols;++j)
os<<std::setw(width)<<m(i,j)<<" ";
if(m.cols>0)
os<<std::setw(width)<<m(i,m.cols-1)<<"\n  ";
}
for(unsigned j=0;j+1<m.cols;++j)
os<<std::setw(width)<<m(m.rows-1,j)<<" ";
if(m.rows>0&&m.cols>0)
os<<std::setw(width)<<m(m.rows-1,m.cols-1);
os<<" ]";
return os;
}
}
template<unsigned M,unsigned N,typename Real>
__forceinline std::istream&operator>>(std::istream&is,static_matrix<M,N,Real>&m)
{
std::string delims="[](), \n";
while(is.good()&&delims.find(is.peek())!=std::string::npos)
is.get();
std::string token;
for(unsigned i=0;i<M*N;++i)
{
if(!heinlib::getline(is,token,delims))
{
for(unsigned j=i;j<M*N;++j)
m[j]=0;
return is;
}
m[i]=convert<Real>(token);
}
return is;
}
#pragma endregion
}
#pragma once
#include<stdexcept>
#pragma once
#include<cstdio>
namespace heinlib
{
template<class Matrix>
typename enable_if<is_matrix<Matrix>,std::string>::type
print_size(const Matrix&m)
{
char txt[25];
sprintf(txt,"%u x %u",m.rows,m.cols);
return std::string(txt);
}
template<typename Matrix>
void resize_if_dynamic(Matrix&m,unsigned M,unsigned N){}
template<typename Real>
void resize_if_dynamic(dynamic_matrix<Real>&m,unsigned M,unsigned N)
{m.resize(M,N);}
template<class Mx1,class Nx1>
struct MxN_result
{
typedef dynamic_matrix<typename Mx1::real_type>result_type;
};
template<unsigned M,unsigned N,class Real>
struct MxN_result<static_matrix<M,1,Real>,static_matrix<N,1,Real> >
{
typedef static_matrix<M,N,Real>result_type;
};
template<typename M1,typename M2>
struct multiply_result;
template<typename Real>
struct multiply_result<dynamic_matrix<Real>,dynamic_matrix<Real> >
{
typedef dynamic_matrix<Real>result_type;
};
template<unsigned M,unsigned N,unsigned P,typename Real>
struct multiply_result<static_matrix<M,N,Real>,static_matrix<N,P,Real> >
{
typedef static_matrix<M,P,Real>result_type;
};
template<unsigned M,unsigned N,typename Real>
struct multiply_result<static_matrix<M,N,Real>,dynamic_matrix<Real> >
{
typedef dynamic_matrix<Real>result_type;
};
template<unsigned M,unsigned N,typename Real>
struct multiply_result<dynamic_matrix<Real>,static_matrix<M,N,Real> >
{
typedef dynamic_matrix<Real>result_type;
};
template<typename M1,typename M2>
struct augment_result;
template<typename Real>
struct augment_result<dynamic_matrix<Real>,dynamic_matrix<Real> >
{
typedef dynamic_matrix<Real>result_type;
};
template<unsigned M,unsigned N,typename Real>
struct augment_result<static_matrix<M,N,Real>,dynamic_matrix<Real> >
{
typedef dynamic_matrix<Real>result_type;
};
template<unsigned M,unsigned N,typename Real>
struct augment_result<dynamic_matrix<Real>,static_matrix<M,N,Real> >
{
typedef dynamic_matrix<Real>result_type;
};
template<unsigned M,unsigned N,unsigned P,typename Real>
struct augment_result<static_matrix<M,N,Real>,static_matrix<M,P,Real> >
{
typedef static_matrix<M,N+P,Real>result_type;
};
template<typename M1,unsigned C>
struct augment_columns;
template<unsigned M,unsigned N,unsigned C,typename Real>
struct augment_columns<static_matrix<M,N,Real>,C>
{
typedef static_matrix<M,N+C,Real>result_type;
};
template<typename Real,unsigned C>
struct augment_columns<dynamic_matrix<Real>,C>
{
typedef dynamic_matrix<Real>result_type;
};
template<typename M1,typename M2>
struct stack_result;
template<typename Real>
struct stack_result<dynamic_matrix<Real>,dynamic_matrix<Real> >
{
typedef dynamic_matrix<Real>result_type;
};
template<unsigned M,unsigned N,typename Real>
struct stack_result<static_matrix<M,N,Real>,dynamic_matrix<Real> >
{
typedef dynamic_matrix<Real>result_type;
};
template<unsigned M,unsigned N,typename Real>
struct stack_result<dynamic_matrix<Real>,static_matrix<M,N,Real> >
{
typedef dynamic_matrix<Real>result_type;
};
template<unsigned M,unsigned N,unsigned P,typename Real>
struct stack_result<static_matrix<M,N,Real>,static_matrix<P,N,Real> >
{
typedef static_matrix<M+P,N,Real>result_type;
};
template<typename M1>
struct square_result;
template<typename Real>
struct square_result<dynamic_matrix<Real> >
{
typedef dynamic_matrix<Real>result_type;
};
template<unsigned M,unsigned N,typename Real>
struct square_result<static_matrix<M,N,Real> >
{
typedef static_matrix<M,M,Real>result_type;
};
template<typename M1>
struct big_square;
template<typename Real>
struct big_square<dynamic_matrix<Real> >
{
typedef dynamic_matrix<Real>result_type;
};
template<unsigned M,unsigned N,typename Real>
struct big_square<static_matrix<M,N,Real> >
{
typedef static_matrix<(M>=N)?M:N,(M>=N)?M:N,Real>result_type;
};
template<typename MI>
struct transpose_result;
template<typename Real>
struct transpose_result<dynamic_matrix<Real> >
{
typedef dynamic_matrix<Real>result_type;
};
template<unsigned M,unsigned N,typename Real>
struct transpose_result<static_matrix<M,N,Real> >
{
typedef static_matrix<N,M,Real>result_type;
};
template<typename MI>
struct column_vector;
template<typename Real>
struct column_vector<dynamic_matrix<Real> >
{
typedef dynamic_matrix<Real>result_type;
};
template<unsigned M,unsigned N,typename Real>
struct column_vector<static_matrix<M,N,Real> >
{
typedef static_matrix<M,1,Real>result_type;
};
template<typename MI>
struct row_vector;
template<typename Real>
struct row_vector<dynamic_matrix<Real> >
{
typedef dynamic_matrix<Real>result_type;
};
template<unsigned M,unsigned N,typename Real>
struct row_vector<static_matrix<M,N,Real> >
{
typedef static_matrix<1,N,Real>result_type;
};
template<typename MI>
struct diagonal_vector;
template<typename Real>
struct diagonal_vector<dynamic_matrix<Real> >
{
typedef dynamic_matrix<Real>result_type;
};
template<unsigned M,unsigned N,typename Real>
struct diagonal_vector<static_matrix<M,N,Real> >
{
typedef static_matrix<((M>N)?M:N),1,Real>result_type;
};
}
namespace heinlib
{
template<class Matrix>
typename enable_if<is_matrix<Matrix>,bool>::type
corrupt(const Matrix&m)
{
unsigned sz=m.rows*m.cols;
for(unsigned i=0;i<sz;++i)
if(corrupt(m[i]))return true;
return false;
}
template<unsigned M,unsigned N,class Real>
__forceinline static_matrix<M-1,1,Real>homogeneous_transform(const static_matrix<M,N,Real>&H,const static_matrix<N-1,1,Real>&X)
{
static_matrix<M,1,Real>HX=H*homogenize(X);
if(HX[M-1]==0)throw std::runtime_error("divide by zero");
return realize(HX);
}
template<class Matrix>
__forceinline typename Matrix::real_type standardize_scale(Matrix&A)
{
typedef typename Matrix::real_type Real;
Real largest_abs=0,largest=0;
unsigned elts=A.rows*A.cols;
for(unsigned i=0;i<elts;++i){
if(abs(A[i])>largest_abs)
{
largest=A[i];
largest_abs=abs(largest);
}
}
if(largest_abs>0)
A/=largest;
return largest;
}
template<class Matrix>
__forceinline Matrix standard_scale(const Matrix&A)
{
typedef typename Matrix::real_type Real;
Real largest_abs=0,largest=0;
unsigned elts=A.rows*A.cols;
for(unsigned i=0;i<elts;++i){
if(abs(A[i])>largest_abs)
{
largest=A[i];
largest_abs=abs(largest);
}
}
if(largest_abs>0)return A/largest;
else return A;
}
template<class Matrix>
__forceinline typename Matrix::real_type scale_factor(const Matrix&A)
{
typedef typename Matrix::real_type Real;
Real largest_abs=0,largest=0;
unsigned elts=A.rows*A.cols;
for(unsigned i=0;i<elts;++i){
if(abs(A[i])>largest_abs)
{
largest=A[i];
largest_abs=abs(largest);
}
}
return largest;
}
template<typename MxN>
unsigned num_elements(const MxN&m)
{
return m.rows*m.cols;
}
template<typename MxN>
typename MxN::real_type sum(const MxN&m)
{
typedef typename MxN::real_type Real;
unsigned elts=num_elements(m);
Real s=0;
for(unsigned i=0;i<elts;++i)
s+=m[i];
return s;
}
template<typename Matrix>
typename enable_if<is_matrix<Matrix>,Matrix>::type
log(const Matrix&Q)
{
typedef typename Matrix::real_type Real;
Matrix res;
resize_if_dynamic(res,Q.rows,Q.cols);
Real theta=acos((trace(Q)-1)/2);
if(theta==0)
{
fill(res,0);
return res;
}else{
res=theta/(2*sin(theta))*(Q-transpose(Q));
return res;
}
}
template<typename MxN>
typename MxN::real_type frobenius_norm(const MxN&m)
{
typedef typename MxN::real_type Real;
unsigned elts=num_elements(m);
Real sum=0;
for(unsigned i=0;i<elts;++i)
sum+=m[i]*m[i];
return sqrt(sum);
}
template<typename MxN>
typename MxN::real_type max_relative_diff(const MxN&a,const MxN&b)
{
typedef typename MxN::real_type Real;
unsigned elts=num_elements(a);
if(num_elements(b)!=elts)
return std::numeric_limits<Real>::infinity();
Real max_rdiff=0;
for(unsigned i=0;i<elts;++i)
max_rdiff=std::max(max_rdiff,abs((a[i]-b[i])/a[i]));
return max_rdiff;
}
template<typename MxN>
typename MxN::real_type max_norm(const MxN&m)
{
typedef typename MxN::real_type Real;
unsigned elts=num_elements(m);
Real maxv=m[0];
for(unsigned i=1;i<elts;++i){
if(abs(m[i])>maxv)
maxv=abs(m[i]);
}
return maxv;
}
template<typename MxN>
typename MxN::real_type max_element(const MxN&m)
{
typedef typename MxN::real_type Real;
unsigned elts=num_elements(m);
Real maxv=m[0];
for(unsigned i=1;i<elts;++i){
if(m[i]>maxv)
maxv=m[i];
}
return maxv;
}
template<typename MxN>
typename MxN::real_type min_element(const MxN&m)
{
typedef typename MxN::real_type Real;
unsigned elts=num_elements(m);
Real minv=m[0];
for(unsigned i=1;i<elts;++i){
if(m[i]<minv)
minv=m[i];
}
return minv;
}
template<typename MxM>
void make_symmetric(MxM&A)
{
assert(is_square(A));
for(unsigned r=1;r<A.rows;++r)
for(unsigned c=0;c<r+1;++c)
A(r,c)=A(c,r);
}
template<unsigned M,unsigned N,typename Real>
static_matrix<N,M,Real>transpose(const static_matrix<M,N,Real>&m)
{
static_matrix<N,M,Real>ret;
for(unsigned i=0;i<M;++i)
for(unsigned j=0;j<N;++j)
ret(j,i)=m(i,j);
return ret;
}
template<typename Real>
dynamic_matrix<Real>transpose(const dynamic_matrix<Real>&m)
{
dynamic_matrix<Real>ret(m.cols,m.rows);
for(unsigned i=0;i<m.rows;++i)
for(unsigned j=0;j<m.cols;++j)
ret(j,i)=m(i,j);
return ret;
}
template<unsigned N,class Real>
static_matrix<N,N,Real>diagonal_square_root(const static_matrix<N,N,Real>&D)
{
static_matrix<N,N,Real>ret={};
for(int i=0;i<N;++i)
ret(i,i)=sqrt(D(i,i));
return ret;
}
template<unsigned M,unsigned N,typename Real>
static_matrix<N,N,Real>transpose_premultiply(const static_matrix<M,N,Real>&m)
{
static_matrix<N,N,Real>ret;
for(unsigned i=0;i<N;++i){
for(unsigned j=0;j<N;++j){
Real sum=0;
for(unsigned k=0;k<M;++k)
sum+=m(k,i)*m(k,j);
ret(i,j)=sum;
ret(j,i)=sum;
}
}
return ret;
}
template<typename Real>
dynamic_matrix<Real>transpose_premultiply(const dynamic_matrix<Real>&m)
{
dynamic_matrix<Real>ret(m.cols,m.cols);
for(unsigned i=0;i<m.cols;++i){
for(unsigned j=0;j<m.cols;++j){
Real sum=0;
for(unsigned k=0;k<m.rows;++k)
sum+=m(k,i)*m(k,j);
ret(i,j)=sum;
ret(j,i)=sum;
}
}
return ret;
}
template<typename M1,typename M2,typename M3>
void elementwise_multiply(const M1&m1,const M2&m2,M3&m3)
{
assert(same_dim(m1,m2));
resize_if_dynamic(m3,m1.rows,m1.cols);
for(unsigned i=0;i<m1.rows*m1.cols;++i)
m3[i]=m1[i]*m2[i];
}
template<typename M1,typename M2>
M1 elementwise_multiply(const M1&m1,const M2&m2)
{
assert(same_dim(m1,m2));
M1 m3;
resize_if_dynamic(m3,m1.rows,m1.cols);
assert(same_dim(m1,m3));
for(unsigned i=0;i<m1.rows*m1.cols;++i)
m3[i]=m1[i]*m2[i];
return m3;
}
template<class Real,unsigned M,unsigned N>
Real matrix_scale(static_matrix<M,N,Real>&A,static_matrix<M,N,Real>&B)
{
Real ata=0,atb=0;
for(unsigned i=0;i<M*N;++i)
{
ata+=A[i]*A[i];
atb+=A[i]*B[i];
}
return atb/ata;
}
template<typename MI>
inline typename MI::real_type trace(const MI&in)
{
assert(is_square(in));
typedef typename MI::real_type Real;
Real sum=0;
for(unsigned i=0;i<in.rows;++i)
sum+=in(i,i);
return sum;
}
template<typename MI>
inline MI&fill(MI&in,typename MI::real_type val)
{
for(unsigned i=0;i<in.rows;++i)
for(unsigned j=0;j<in.cols;++j)
in(i,j)=val;
return in;
}
template<typename MI,typename MO>
void matrix_copy(MO&out,const MI&in)
{
assert(in.rows*in.cols==out.rows*out.cols);
for(unsigned i=0;i<in.rows*in.cols;++i)
out[i]=in[i];
}
template<typename MI,typename MO>
void matrix_copy(MO&out,const MI&in,unsigned count)
{
assert(in.rows*in.cols>=count);
assert(out.rows*out.cols>=count);
for(unsigned i=0;i<count;++i)
out[i]=in[i];
}
template<typename MI>
inline MI&fill_identity(MI&in)
{
for(unsigned i=0;i<in.rows;++i){
for(unsigned j=0;j<in.cols;++j)
in(i,j)=0;
in(i,i)=1;
}
return in;
}
template<typename MI>
inline MI&fill_diagonal(MI&in,typename MI::real_type val)
{
for(unsigned i=0;i<in.rows;++i){
for(unsigned j=0;j<in.cols;++j)
in(i,j)=0;
in(i,i)=val;
}
return in;
}
template<unsigned N,typename Real>
inline static_matrix<N,N,Real>identity()
{
static_matrix<N,N,Real>ret={0};
for(unsigned i=0;i<N;++i)
ret(i,i)=1;
return ret;
}
template<unsigned Dim,class Real>
inline static_matrix<Dim,Dim,Real>make_diagonal(const Real&s)
{
static_matrix<Dim,Dim,Real>m={};
for(int i=0;i<Dim;++i)
m(i,i)=s;
return m;
}
template<typename Nx1>
inline typename square_result<Nx1>::result_type make_diagonal(const Nx1&diag_elts)
{
unsigned N=diag_elts.rows;
typename square_result<Nx1>::result_type ret;
resize_if_dynamic(ret,N,N);
fill(ret,0);
for(unsigned i=0;i<N;++i)
ret(i,i)=diag_elts[i];
return ret;
}
template<typename NewType,unsigned M,unsigned N,typename Real>
__forceinline typename disable_if<is_same_type<NewType,Real>,static_matrix<M,N,NewType> >::type
change_type(const static_matrix<M,N,Real>&m)
{
static_matrix<M,N,NewType>ret;
for(unsigned i=0;i<M*N;++i)
ret.data[i]=(NewType)(m.data[i]);
return ret;
}
template<typename NewType,unsigned M,unsigned N,typename Real>
__forceinline typename enable_if<is_same_type<NewType,Real>,static_matrix<M,N,NewType> >::type
change_type(const static_matrix<M,N,Real>&m)
{
return m;
}
template<typename NewType,typename Real>
__forceinline typename disable_if<is_same_type<NewType,Real>,dynamic_matrix<NewType> >::type
change_type(const dynamic_matrix<Real>&m)
{
dynamic_matrix<NewType>ret(m.rows*m.cols);
for(unsigned i=0;i<m.rows*m.cols;++i)
ret.data[i]=(NewType)(m.data[i]);
return ret;
}
template<typename NewType,typename Real>
__forceinline typename enable_if<is_same_type<NewType,Real>,dynamic_matrix<NewType> >::type
change_type(const dynamic_matrix<Real>&m)
{
return m;
}
}
#pragma once
namespace heinlib
{
template<typename Vector>struct dot_impl
{
__forceinline static typename Vector::real_type call(const Vector&in,const Vector&in2)
{
typedef typename Vector::real_type Real;
unsigned N=in.rows*in.cols;
Real sum=0;
for(unsigned i=0;i<N;++i)
sum+=in[i]*in2[i];
return sum;
}
};
template<typename Real>struct dot_impl<static_matrix<1,1,Real> >
{
__forceinline static Real call(const static_matrix<1,1,Real>&in,const static_matrix<1,1,Real>&in2)
{
return in[0]*in2[0];
}
};
template<typename Real>struct dot_impl<static_matrix<2,1,Real> >
{
__forceinline static Real call(const static_matrix<2,1,Real>&in,const static_matrix<2,1,Real>&in2)
{
return in[0]*in2[0]+in[1]*in2[1];
}
};
template<typename Real>struct dot_impl<static_matrix<3,1,Real> >
{
__forceinline static Real call(const static_matrix<3,1,Real>&in,const static_matrix<3,1,Real>&in2)
{
return in[2]*in2[2]+in[0]*in2[0]+in[1]*in2[1];
}
};
template<typename Real>struct dot_impl<static_matrix<4,1,Real> >
{
__forceinline static Real call(const static_matrix<4,1,Real>&in,const static_matrix<4,1,Real>&in2)
{
return in[3]*in2[3]+in[0]*in2[0]+in[1]*in2[1]+in[2]*in2[2];
}
};
template<typename Vector>
__forceinline typename Vector::real_type dot(const Vector&in,const Vector&in2)
{
return dot_impl<Vector>::call(in,in2);
}
template<typename Real>
__forceinline Real perp_dot(const static_matrix<2,1,Real>&v1,const static_matrix<2,1,Real>&v2)
{
return v1.x()*v2.y()-v1.y()*v2.x();
}
}
#pragma once
namespace heinlib
{
namespace detail
{
template<typename Vector>
struct length_sq_impl
{
__forceinline static typename Vector::real_type call(const Vector&in)
{
typedef typename Vector::real_type Real;
unsigned N=in.rows*in.cols;
Real dist=0;
for(unsigned i=0;i<N;++i)
dist+=in[i]*in[i];
return dist;
}
};
template<typename Real>
struct length_sq_impl<static_matrix<1,1,Real> >
{
__forceinline static Real call(const static_matrix<1,1,Real>&in)
{
return in[0]*in[0];
}
};
template<typename Real>
struct length_sq_impl<static_matrix<2,1,Real> >
{
__forceinline static Real call(const static_matrix<2,1,Real>&in)
{
return in[0]*in[0]+in[1]*in[1];
}
};
template<typename Real>
struct length_sq_impl<static_matrix<3,1,Real> >
{
__forceinline static Real call(const static_matrix<3,1,Real>&in)
{
return in[2]*in[2]+in[0]*in[0]+in[1]*in[1];
}
};
template<typename Real>
struct length_sq_impl<static_matrix<4,1,Real> >
{
__forceinline static Real call(const static_matrix<4,1,Real>&in)
{
return in[3]*in[3]+in[0]*in[0]+in[1]*in[1]+in[2]*in[2];
}
};
}
template<typename Vector>
__forceinline typename Vector::real_type length_sq(const Vector&in)
{
return detail::length_sq_impl<Vector>::call(in);
}
template<typename Vector>
__forceinline typename Vector::real_type length(const Vector&in)
{
return sqrt(detail::length_sq_impl<Vector>::call(in));
}
template<typename Vector>
__forceinline float lengthf(const Vector&in)
{
return sqrt((float)detail::length_sq_impl<Vector>::call(in));
}
template<typename Vector>
__forceinline typename Vector::real_type distance_sq(const Vector&in,const Vector&in2)
{
return detail::length_sq_impl<Vector>::call(in-in2);
}
template<typename Vector>
__forceinline typename Vector::real_type distance(const Vector&in,const Vector&in2)
{
return sqrt(detail::length_sq_impl<Vector>::call(in-in2));
}
template<typename Vector>
__forceinline float distancef(const Vector&in,const Vector&in2)
{
return sqrt((float)detail::length_sq_impl<Vector>::call(in-in2));
}
}
#pragma once
namespace heinlib
{
template<unsigned N,class Real>
inline typename enable_if<is_floating<Real>,Real>::type
angle(const static_matrix<N,1,Real>&a,const static_matrix<N,1,Real>&b)
{
Real arg=dot(a,b)/(length(a)*length(b));
return(abs(arg)<1)?acos(arg):0;
}
template<unsigned N,class Real>
inline typename disable_if<is_floating<Real>,float>::type
angle(const static_matrix<N,1,Real>&a,const static_matrix<N,1,Real>&b)
{
float arg=dot(a,b)/(float)(length(a)*length(b));
return(abs(arg)<1)?acos(arg):0;
}
template<class Real>
inline Real signed_angle(const static_matrix<2,1,Real>&a,const static_matrix<2,1,Real>&b)
{
return atan2(perp_dot(a,b),dot(a,b));
}
template<class Real>
inline Real angle(const static_matrix<2,1,Real>&in)
{
Real a=atan2(in.y(),in.x());
return(a>=0)?a:(a+constants<Real>::two_pi());
}
}
#pragma once
#pragma once
#include<cmath>
#include<algorithm>
#include<iomanip>
namespace heinlib
{
template<int Loud,typename MxN>
bool gauss_jordan_elimination(MxN&R,typename MxN::real_type eps=0)
{
typedef typename MxN::real_type Real;
unsigned M=R.rows,N=R.cols;
assert(N>=M);
if(Loud>0)std::cout<<"Start:\n"<<R<<"\n";
if(!partial_pivoting(R))
return false;
if(Loud>0)std::cout<<"Pivoted:\n"<<R<<"\n";
unsigned c=0;
for(unsigned r=0;r<M;++r)
{
while(c<N&&abs(R(r,c))<=eps)++c;
if(c==N)return true;
Real pivot=R(r,c);
for(unsigned j=r+1;j<M;++j)
{
Real m=R(j,c)/pivot;
R(j,c)=0;
for(unsigned k=c+1;k<N;++k)
R(j,k)-=m*R(r,k);
}
for(unsigned j=0;j<r;++j)
{
Real m=R(j,c)/pivot;
R(j,c)=0;
for(unsigned k=c+1;k<N;++k)
R(j,k)-=m*R(r,k);
}
R(r,c)=1;
for(unsigned k=c+1;k<N;++k)
R(r,k)/=pivot;
++c;
if(Loud>0){
std::cout<<"Step:\n"<<R<<"\n";
system("pause");
}
}
return true;
}
template<int Loud,typename MxN>
bool gauss_jordan_elimination(const MxN&A,MxN&R,typename MxN::real_type eps=0)
{
unsigned M=A.rows,N=A.cols;
assert(N>=M);
resize_if_dynamic(R,M,N);
assert(same_dim(A,R));
typedef typename MxN::real_type Real;
R=A;
return gauss_jordan_elimination<Loud>(R,eps);
}
template<typename MxM,typename Mx1>
bool gaussian_elimination(const MxM&A,const Mx1&b,Mx1&x)
{
unsigned M=A.rows;
typedef typename MxM::real_type Real;
assert(size_is(A,M,M));
assert(size_is(b,M,1));
resize_if_dynamic(x,M,1);
assert(size_is(x,M,1));
typename augment_result<MxM,Mx1>::result_type R;
resize_if_dynamic(R,M,M+1);
R.block_insert(0,0,A);
R.block_insert(0,M,b);
if(!partial_pivoting(R))
return false;
for(unsigned d=0;d<M;++d)
{
for(unsigned j=d+1;j<M;++j)
{
Real pivot=R(d,d);
Real m=R(j,d)/pivot;
for(unsigned p=d;p<R.cols;++p)
R(j,p)-=m*R(d,p);
for(unsigned p=0;p<=d;++p)
R(j,p)=0;
}
}
back_substitution(R,x);
return true;
}
template<typename MxN,typename Mx1,typename Nx1>
void back_substitution(const MxN&U,const Mx1&y,Nx1&x)
{
unsigned M=U.rows,N=U.cols;
typedef typename MxN::real_type Real;
assert(size_is(U,M,N));
assert(size_is(y,M,1));
resize_if_dynamic(x,N,1);
assert(size_is(x,N,1));
x[N-1]=y[N-1]/U(N-1,N-1);
for(int i=N-2;i>=0;--i)
{
Real sum=0;
for(unsigned j=i+1;j<N;++j)
sum+=U(i,j)*x[j];
x[i]=(y[i]-sum)/U(i,i);
}
}
template<typename M1,typename M2>
void back_substitution(const M1&aug,M2&x)
{
typedef typename M1::real_type Real;
unsigned N=aug.rows;
assert(size_is(aug,N,N+1));
resize_if_dynamic(x,N,1);
assert(size_is(x,N,1));
x[N-1]=aug(N-1,N)/aug(N-1,N-1);
for(int i=N-2;i>=0;--i)
{
Real sum=0;
for(unsigned j=i+1;j<N;++j)
sum+=aug(i,j)*x[j];
x[i]=(aug(i,N)-sum)/aug(i,i);
}
}
template<typename NxN,typename Nx1>
void forward_substitution(const NxN&L,const Nx1&b,Nx1&y)
{
typedef typename NxN::real_type Real;
unsigned N=L.rows;
assert(is_square(L));
assert(size_is(b,N,1));
resize_if_dynamic(y,N,1);
assert(size_is(y,N,1));
y[0]=b[0]/L(0,0);
for(unsigned i=1;i<N;++i){
Real sum=0;
for(unsigned j=0;j<i;++j)
sum+=L(i,j)*y[j];
y[i]=(b[i]-sum)/L(i,i);
}
}
template<typename M1,typename M2>
void forward_substitution(const M1&aug,M2&y)
{
typedef typename M1::real_type Real;
unsigned N=aug.rows;
assert(size_is(aug,N,N+1));
resize_if_dynamic(y,N,1);
assert(size_is(y,N,1));
y[0]=aug(0,N)/aug(0,0);
for(unsigned i=1;i<N;++i){
Real sum=0;
for(unsigned j=0;j<i;++j)
sum+=aug(i,j)*y[j];
y[i]=(aug(i,N)-sum)/aug(i,i);
}
}
template<typename MxN,typename MxM>
bool partial_pivoting(MxN&aug,MxM&P)
{
typedef typename MxN::real_type Real;
unsigned M=aug.rows,N=aug.cols;
assert(N>=M);
assert(size_is(P,M,M));
fill(P,0);
for(unsigned d=0;d<M;++d)
P(d,d)=1;
for(unsigned d=0;d<M;++d)
{
unsigned argmax_r=d;
Real max_v=abs(aug(d,d));
for(unsigned r=d+1;r<M;++r){
Real v=abs(aug(r,d));
if(v>max_v){
max_v=v;
argmax_r=r;
}
}
if(max_v==0)
return false;
if(d!=argmax_r){
for(unsigned c=0;c<N;++c)
std::swap(aug(d,c),aug(argmax_r,c));
if(P!=NULL){
for(unsigned c=0;c<M;++c){
std::swap(P(d,c),P(argmax_r,c));
}
}
}
}
return true;
}
template<typename MxN>
bool partial_pivoting(MxN&aug)
{
typedef typename MxN::real_type Real;
unsigned M=aug.rows,N=aug.cols;
assert(N>=M);
for(unsigned d=0;d<M;++d)
{
unsigned argmax_r=d;
Real max_v=abs(aug(d,d));
for(unsigned r=d+1;r<M;++r){
Real v=abs(aug(r,d));
if(v>max_v){
max_v=v;
argmax_r=r;
}
}
if(d!=argmax_r)
for(unsigned c=0;c<N;++c)
std::swap(aug(d,c),aug(argmax_r,c));
}
return true;
}
template<typename MxN>
bool scaled_partial_pivoting(MxN&aug)
{
typedef typename MxN::real_type Real;
unsigned M=aug.rows,N=aug.cols;
assert(N>=M);
typename column_vector<MxN>::result_type scale;
for(unsigned i=0;i<M;++i){
Real v=abs(aug(i,0));
for(unsigned j=1;j<N;++j)
v=std::max(v,abs(aug(i,j)));
scale[i]=v;
}
for(unsigned d=0;d<M;++d)
{
unsigned argmax_r=d;
Real max_v=abs(aug(d,d))/scale[d];
for(unsigned r=d+1;r<M;++r){
Real v=abs(aug(r,d))/scale[d];
if(v>max_v){
max_v=v;
argmax_r=r;
}
}
if(d!=argmax_r)
for(unsigned c=0;c<N;++c)
std::swap(aug(d,c),aug(argmax_r,c));
}
return true;
}
template<typename NxN,typename Nx1>
bool gauss_seidel_iterations(const NxN&A,const Nx1&b,Nx1&x,
typename NxN::real_type stop_eps,
unsigned max_iters=1,unsigned min_iters=1)
{
using namespace heinlib;
typedef typename NxN::real_type Real;
unsigned N=A.rows;
assert(size_is(b,N,1));
resize_if_dynamic(x,N,1);
assert(size_is(x,N,1));
typename augment_result<NxN,Nx1>::result_type aug;
resize_if_dynamic(aug,N,N+1);
aug.block_insert(0,0,A);
aug.block_insert(0,N,b);
for(unsigned iter=0;iter<max_iters;++iter)
{
Real maxdiff=0;
for(unsigned j=0;j<N;++j)
{
Real sum1=0,sum2=0;
for(unsigned k=0;k<j;++k)
sum1+=aug(j,k)*x[k];
for(unsigned k=j+1;k<N;++k)
sum2+=aug(j,k)*x[k];
Real xn=(aug(j,N)-sum1-sum2)/aug(j,j);
Real diff=abs(x[j]-xn);
x[j]=xn;
if(diff>maxdiff)
maxdiff=diff;
}
if(iter>min_iters&&maxdiff<stop_eps)
return true;
}
return false;
}
}
#pragma once
#include<limits>
namespace heinlib
{
#ifdef WIN32
template<bool WANT_U,bool WANT_V,typename MxN,typename NxN>
bool SVD(const MxN&Arg,MxN&U,NxN&S,NxN&V,typename MxN::real_type eps,unsigned max_iters);
#else
template<bool WANT_U,bool WANT_V,typename MxN,typename NxN>
bool SVD(const MxN&Arg,MxN&U,NxN&S,NxN&V,typename MxN::real_type eps=2.2e-10,unsigned max_iters=1000);
#endif
namespace detail
{
template<bool WANT_U,bool WANT_V,typename MxN,typename NxN>
bool SVD_pad(const MxN&Arg,MxN&U,NxN&S,NxN&V,
typename MxN::real_type eps,unsigned max_iters)
{
typedef typename MxN::real_type Real;
unsigned M=Arg.rows,N=Arg.cols;
assert(M<N);
NxN APad;
resize_if_dynamic(APad,N,N);
fill(APad,0);
APad.block_insert(0,0,Arg);
NxN UPad,SPad,VPad;
resize_if_dynamic(SPad,N,N);
if(WANT_U)resize_if_dynamic(UPad,N,N);
if(WANT_V)resize_if_dynamic(VPad,N,N);
bool ret=SVD<WANT_U,WANT_V>(APad,UPad,SPad,VPad,eps,max_iters);
matrix_copy(S,SPad,M*N);
for(unsigned i=M;i<N;++i)
{
for(unsigned j=0;j<N;++j)
S(i,j)=0;
}
if(WANT_U)matrix_copy(U,UPad,M*N);
if(WANT_V)matrix_copy(V,VPad,N*N);
return ret;
}
}
template<bool WANT_U,bool WANT_V,typename MxN,typename NxN>
bool SVD(const MxN&Arg,MxN&U,NxN&S,NxN&V,
typename MxN::real_type eps=2.2e-10,
unsigned max_iters=1000)
{
typedef typename MxN::real_type Real;
int M=Arg.rows,N=Arg.cols;
if(WANT_V&&M<N)
{
return detail::SVD_pad<WANT_U,WANT_V>(Arg,U,S,V,eps,max_iters);
}
#ifndef NDEBUG
for(int i=0;i<M*N;++i)
{
if(corrupt(Arg[i]))
{
std::cout<<"SVD Corrupt! M = \n"<<Arg<<"\n";
return false;
}
}
#endif
resize_if_dynamic(U,M,N);
resize_if_dynamic(S,N,N);
resize_if_dynamic(V,N,N);
assert(size_is(S,N,N));
assert(size_is(V,N,N));
fill(U,0);
fill(S,0);
MxN A=Arg;
typename column_vector<NxN>::result_type s,e;
typename column_vector<MxN>::result_type work;
resize_if_dynamic(s,N,1);
resize_if_dynamic(e,N,1);
resize_if_dynamic(work,M,1);
int m=M;
int n=N;
int nu=std::min(M,N);
int i=0,j=0,k=0;
int nct=std::min(M-1,N);
int nrt=std::max(0,std::min(N-2,M));
for(k=0;k<std::max(nct,nrt);k++){
if(k<nct){
s[k]=0;
for(i=k;i<m;i++){
s[k]=hypot(s[k],A(i,k));
}
if(s[k]!=0){
if(A(k,k)<0){
s[k]=-s[k];
}
for(i=k;i<m;i++){
A(i,k)/=s[k];
}
A(k,k)+=1;
}
s[k]=-s[k];
}
for(j=k+1;j<n;j++){
if((k<nct)&&(s[k]!=0)){
Real t(0);
for(i=k;i<m;i++){
t+=A(i,k)*A(i,j);
}
t=-t/A(k,k);
for(i=k;i<m;i++){
A(i,j)+=t*A(i,k);
}
}
e[j]=A(k,j);
}
if(WANT_U&(k<nct)){
for(i=k;i<m;i++){
U(i,k)=A(i,k);
}
}
if(k<nrt){
e[k]=0;
for(i=k+1;i<n;i++){
e[k]=hypot(e[k],e[i]);
}
if(e[k]!=0){
if(e[k+1]<0){
e[k]=-e[k];
}
for(i=k+1;i<n;i++){
e[i]/=e[k];
}
e[k+1]+=1;
}
e[k]=-e[k];
if((k+1<m)&(e[k]!=0)){
for(i=k+1;i<m;i++){
work[i]=0;
}
for(j=k+1;j<n;j++){
for(i=k+1;i<m;i++){
work[i]+=e[j]*A(i,j);
}
}
for(j=k+1;j<n;j++){
Real t(-e[j]/e[k+1]);
for(i=k+1;i<m;i++){
A(i,j)+=t*work[i];
}
}
}
if(WANT_V){
for(i=k+1;i<n;i++){
V(i,k)=e[i];
}
}
}
}
int p=std::min(n,m+1);
if(nct<n){
s[nct]=A(nct,nct);
}
if(m<p){
s[p-1]=0;
}
if(nrt+1<p){
e[nrt]=A(nrt,p-1);
}
e[p-1]=0;
if(WANT_U){
for(j=nct;j<nu;j++){
for(i=0;i<m;i++){
U(i,j)=0;
}
U(j,j)=1;
}
for(k=nct-1;k>=0;k--){
if(s[k]!=0){
for(j=k+1;j<nu;j++){
Real t(0);
for(i=k;i<m;i++){
t+=U(i,k)*U(i,j);
}
t=-t/U(k,k);
for(i=k;i<m;i++){
U(i,j)+=t*U(i,k);
}
}
for(i=k;i<m;i++){
U(i,k)=-U(i,k);
}
U(k,k)=1+U(k,k);
for(i=0;i<k-1;i++){
U(i,k)=0;
}
}else{
for(i=0;i<m;i++){
U(i,k)=0;
}
U(k,k)=1;
}
}
}
if(WANT_V){
for(k=n-1;k>=0;k--){
if((k<nrt)&(e[k]!=0)){
for(j=k+1;j<nu;j++){
Real t(0);
for(i=k+1;i<n;i++){
t+=V(i,k)*V(i,j);
}
t=-t/V(k+1,k);
for(i=k+1;i<n;i++){
V(i,j)+=t*V(i,k);
}
}
}
for(i=0;i<n;i++){
V(i,k)=0;
}
V(k,k)=1;
}
}
int pp=p-1;
int iter=0;
while(p>0){
int k=0;
int kase=0;
if(iter>(int)max_iters)
{
#ifndef NDEBUG
std::cout<<"SVD stuck in infinite loop!\n";
std::cout<<"arg is "<<Arg.rows<<" x "<<Arg.cols<<"\n";
#endif
return false;
}
for(k=p-2;k>=-1;k--){
if(k==-1){
break;
}
if(abs(e[k])<=eps*(abs(s[k])+abs(s[k+1]))){
e[k]=0;
break;
}
}
if(k==p-2){
kase=4;
}else{
int ks;
for(ks=p-1;ks>=k;ks--){
if(ks==k){
break;
}
Real t((ks!=p?abs(e[ks]):0)+
(ks!=k+1?abs(e[ks-1]):0));
if(abs(s[ks])<=eps*t){
s[ks]=0;
break;
}
}
if(ks==k){
kase=3;
}else if(ks==p-1){
kase=1;
}else{
kase=2;
k=ks;
}
}
k++;
switch(kase){
case 1:{
Real f(e[p-2]);
e[p-2]=0;
for(j=p-2;j>=k;j--){
Real t(hypot(s[j],f));
Real cs(s[j]/t);
Real sn(f/t);
s[j]=t;
if(j!=k){
f=-sn*e[j-1];
e[j-1]=cs*e[j-1];
}
if(WANT_V){
for(i=0;i<n;i++){
t=cs*V(i,j)+sn*V(i,p-1);
V(i,p-1)=-sn*V(i,j)+cs*V(i,p-1);
V(i,j)=t;
}
}
}
}
break;
case 2:{
Real f(e[k-1]);
e[k-1]=0;
for(j=k;j<p;j++){
Real t(hypot(s[j],f));
Real cs(s[j]/t);
Real sn(f/t);
s[j]=t;
f=-sn*e[j];
e[j]=cs*e[j];
if(WANT_U){
for(i=0;i<m;i++){
t=cs*U(i,j)+sn*U(i,k-1);
U(i,k-1)=-sn*U(i,j)+cs*U(i,k-1);
U(i,j)=t;
}
}
}
}
break;
case 3:{
Real scale=std::max(std::max(std::max(std::max(
abs(s[p-1]),
abs(s[p-2])),
abs(e[p-2])),
abs(s[k])),
abs(e[k]));
Real sp=s[p-1]/scale;
Real spm1=s[p-2]/scale;
Real epm1=e[p-2]/scale;
Real sk=s[k]/scale;
Real ek=e[k]/scale;
Real b=((spm1+sp)*(spm1-sp)+epm1*epm1)/2;
Real c=(sp*epm1)*(sp*epm1);
Real shift=0;
if((b!=0)||(c!=0)){
shift=sqrt(b*b+c);
if(b<0){
shift=-shift;
}
shift=c/(b+shift);
}
Real f=(sk+sp)*(sk-sp)+shift;
Real g=sk*ek;
for(j=k;j<p-1;j++){
Real t=hypot(f,g);
Real cs=f/t;
Real sn=g/t;
if(j!=k){
e[j-1]=t;
}
f=cs*s[j]+sn*e[j];
e[j]=cs*e[j]-sn*s[j];
g=sn*s[j+1];
s[j+1]=cs*s[j+1];
if(WANT_V){
for(i=0;i<n;i++){
t=cs*V(i,j)+sn*V(i,j+1);
V(i,j+1)=-sn*V(i,j)+cs*V(i,j+1);
V(i,j)=t;
}
}
t=hypot(f,g);
cs=f/t;
sn=g/t;
s[j]=t;
f=cs*e[j]+sn*s[j+1];
s[j+1]=-sn*e[j]+cs*s[j+1];
g=sn*e[j+1];
e[j+1]=cs*e[j+1];
if(WANT_U&&(j<m-1)){
for(i=0;i<m;i++){
t=cs*U(i,j)+sn*U(i,j+1);
U(i,j+1)=-sn*U(i,j)+cs*U(i,j+1);
U(i,j)=t;
}
}
}
e[p-2]=f;
iter=iter+1;
}
break;
case 4:{
if(s[k]<=0){
s[k]=(s[k]<0?-s[k]:0);
if(WANT_V){
for(i=0;i<=pp;i++){
V(i,k)=-V(i,k);
}
}
}
while(k<pp){
if(s[k]>=s[k+1]){
break;
}
Real t=s[k];
s[k]=s[k+1];
s[k+1]=t;
if(WANT_V&&(k<n-1)){
for(i=0;i<n;i++){
t=V(i,k+1);V(i,k+1)=V(i,k);V(i,k)=t;
}
}
if(WANT_U&&(k<m-1)){
for(i=0;i<m;i++){
t=U(i,k+1);U(i,k+1)=U(i,k);U(i,k)=t;
}
}
k++;
}
iter=0;
p--;
}
break;
}
for(int i=0;i<nu;++i)
S(i,i)=s[i];
}
return true;
}
}
#pragma once
#pragma once
namespace heinlib
{
template<typename NxN>
bool LU_decomposition(const NxN&A,NxN&L,NxN&U)
{
typedef typename NxN::real_type Real;
unsigned N=A.rows;
assert(size_is(A,N,N));
resize_if_dynamic(L,N,N);
resize_if_dynamic(U,N,N);
assert(size_is(L,N,N));
assert(size_is(U,N,N));
U=A;
fill(L,0.0f);
for(unsigned d=0;d<N;++d)
{
L(d,d)=1;
for(unsigned j=d+1;j<N;++j)
{
Real pivot=U(d,d);
if(pivot==0)return false;
Real m=U(j,d)/pivot;
for(unsigned p=d;p<U.cols;++p)
U(j,p)-=m*U(d,p);
for(unsigned p=0;p<d;++p)
U(j,p)=0;
L(j,d)=m;
}
}
for(unsigned p=0;p<N-1;++p)
U(N-1,p)=0;
return true;
}
}
namespace heinlib
{
template<typename MI,typename MO>struct matrix_inverse_impl;
template<unsigned N,typename Real>struct matrix_inverse_impl<static_matrix<N,N,Real>,static_matrix<N,N,Real> >{
static inline bool call(const static_matrix<N,N,Real>&in,
static_matrix<N,N,Real>&out,Real*det)
{
heinlib::static_matrix<N,N*2,Real>aug={0},rre;
aug.block_insert(0,0,in);
for(unsigned i=0;i<N;++i)
aug(i,N+i)=1;
if(gauss_jordan_elimination<0>(aug,rre))
{
out.block_insert(0,0,0,N,rre);
if(det!=NULL)
{
*det=determinant(in);
return true;
}
return true;
}else return false;
}
};
template<typename Real>struct matrix_inverse_impl<dynamic_matrix<Real>,dynamic_matrix<Real> >{
static inline bool call(const dynamic_matrix<Real>&in,
dynamic_matrix<Real>&out,Real*det)
{
assert(is_square(in));
resize_if_dynamic(out,in.rows,in.cols);
assert(same_dim(in,out));
unsigned N=in.rows;
dynamic_matrix<Real>aug(in.rows,in.cols*2),rre(aug.rows,aug.cols);
fill(aug,0);
aug.block_insert(0,0,in);
for(unsigned i=0;i<N;++i)
aug(i,N+i)=1;
if(gauss_jordan_elimination<0>(aug,rre))
{
out.block_insert(0,0,0,N,rre);
if(det!=NULL)
{
*det=determinant(in);
return true;
}
return true;
}else return false;
}
};
template<typename MI,typename MO>struct matrix_inverse_sym_impl{
static inline bool call(const MI&in,MO&out,typename MI::real_type*det)
{
matrix_inverse_sym_impl<MI,MO>::call(in,out,det);
}
};
template<typename Real>struct matrix_inverse_impl<static_matrix<1,1,Real>,static_matrix<1,1,Real> >
{
static inline bool call(const static_matrix<1,1,Real>&in,static_matrix<1,1,Real>&out,Real*det)
{
out[0]=1/in[0];
if(det!=NULL)*det=in[0];
return true;
}
};
template<typename Real>struct matrix_inverse_impl<static_matrix<2,2,Real>,static_matrix<2,2,Real> >
{
static inline bool call(const static_matrix<2,2,Real>&in,static_matrix<2,2,Real>&out,Real*det)
{
Real d=in[0]*in[3]-in[1]*in[2];
if(det!=NULL)*det=d;
if(d!=0)
{
Real d_inv=1/d;
out[0]=in[3]*d_inv;
out[3]=in[0]*d_inv;
out[1]=-in[1]*d_inv;
out[2]=-in[2]*d_inv;
return true;
}
return false;
}
};
template<typename Real>struct matrix_inverse_sym_impl<static_matrix<2,2,Real>,static_matrix<2,2,Real> >
{
static inline bool call(const static_matrix<2,2,Real>&in,static_matrix<2,2,Real>&out,Real*det)
{
Real d=in[0]*in[3]-sq(in[1]);
if(det!=NULL)*det=d;
if(d!=0)
{
Real d_inv=1/d;
out[0]=in[3]*d_inv;
out[3]=in[0]*d_inv;
float t=-in[1]*d_inv;
out[1]=t;
out[2]=t;
return true;
}
return false;
}
};
template<typename Real>struct matrix_inverse_impl<static_matrix<3,3,Real>,static_matrix<3,3,Real> >{
static inline bool call(const static_matrix<3,3,Real>&in,static_matrix<3,3,Real>&out,Real*det)
{
const Real&m0=in[0],&m1=in[1],&m2=in[2],
&m3=in[3],&m4=in[4],&m5=in[5],
&m6=in[6],&m7=in[7],&m8=in[8];
Real A0=m4*m8,
A1=m3*m8,
A2=m3*m7,
B0=m5*m7,
B1=m5*m6,
B2=m4*m6;
Real K0=A0-B0,
K1=A1-B1,
K2=A2-B2;
Real d=m0*K0-m1*K1+m2*K2;
if(det!=NULL)*det=d;
if(d!=0){
Real detInv=1/d;
out[0]=K0*detInv;
out[3]=-K1*detInv;
out[6]=K2*detInv;
out[1]=(m2*m7-m1*m8)*detInv;
out[4]=(m0*m8-m2*m6)*detInv;
out[7]=(m1*m6-m0*m7)*detInv;
out[2]=(m1*m5-m2*m4)*detInv;
out[5]=(m2*m3-m0*m5)*detInv;
out[8]=(m0*m4-m1*m3)*detInv;
return true;
}
return false;
}
};
template<typename Real>struct matrix_inverse_sym_impl<static_matrix<3,3,Real>,static_matrix<3,3,Real> >{
static inline bool call(const static_matrix<3,3,Real>&in,static_matrix<3,3,Real>&out,Real*det)
{
const Real&m0=in[0],&m1=in[1],&m2=in[2],
&m3=in[3],&m4=in[4],&m5=in[5],
&m6=in[6],&m7=in[7],&m8=in[8];
Real A0=m4*m8,
A1=m1*m8,
A2=m1*m5,
B0=m5*m5,
B1=m5*m2,
B2=m4*m2;
Real K0=A0-B0,
K1=A1-B1,
K2=A2-B2;
Real d=m0*K0-m1*K1+m2*K2;
if(det!=NULL)*det=d;
if(d!=0){
Real detInv=1/d;
out[0]=K0*detInv;
out[1]=out[3]=-K1*detInv;
out[2]=out[6]=K2*detInv;
out[4]=(m0*m8-m2*m2)*detInv;
out[5]=out[7]=(m1*m2-m0*m5)*detInv;
out[8]=(m0*m4-m1*m1)*detInv;
return true;
}
return false;
}
};
template<typename Real>struct matrix_inverse_impl<static_matrix<4,4,Real>,static_matrix<4,4,Real> >{
static inline bool call(const static_matrix<4,4,Real>&in,static_matrix<4,4,Real>&out,Real*det)
{
Real A0=in[0]*in[5]-in[1]*in[4];
Real A1=in[0]*in[6]-in[2]*in[4];
Real A2=in[0]*in[7]-in[3]*in[4];
Real A3=in[1]*in[6]-in[2]*in[5];
Real A4=in[1]*in[7]-in[3]*in[5];
Real A5=in[2]*in[7]-in[3]*in[6];
Real B0=in[8]*in[13]-in[9]*in[12];
Real B1=in[8]*in[14]-in[10]*in[12];
Real B2=in[8]*in[15]-in[11]*in[12];
Real B3=in[9]*in[14]-in[10]*in[13];
Real B4=in[9]*in[15]-in[11]*in[13];
Real B5=in[10]*in[15]-in[11]*in[14];
Real d=A0*B5-A1*B4+A2*B3+A3*B2-A4*B1+A5*B0;
if(det!=NULL)*det=d;
if(d!=0)
{
Real invD=1/d;
out[0]=(in[5]*B5-in[6]*B4+in[7]*B3)*invD;
out[4]=(-in[4]*B5+in[6]*B2-in[7]*B1)*invD;
out[8]=(in[4]*B4-in[5]*B2+in[7]*B0)*invD;
out[12]=(-in[4]*B3+in[5]*B1-in[6]*B0)*invD;
out[1]=(-in[1]*B5+in[2]*B4-in[3]*B3)*invD;
out[5]=(in[0]*B5-in[2]*B2+in[3]*B1)*invD;
out[9]=(-in[0]*B4+in[1]*B2-in[3]*B0)*invD;
out[13]=(in[0]*B3-in[1]*B1+in[2]*B0)*invD;
out[2]=(in[13]*A5-in[14]*A4+in[15]*A3)*invD;
out[6]=(-in[12]*A5+in[14]*A2-in[15]*A1)*invD;
out[10]=(in[12]*A4-in[13]*A2+in[15]*A0)*invD;
out[14]=(-in[12]*A3+in[13]*A1-in[14]*A0)*invD;
out[3]=(-in[9]*A5+in[10]*A4-in[11]*A3)*invD;
out[7]=(in[8]*A5-in[10]*A2+in[11]*A1)*invD;
out[11]=(-in[8]*A4+in[9]*A2-in[11]*A0)*invD;
out[15]=(in[8]*A3-in[9]*A1+in[10]*A0)*invD;
return true;
}
return false;
}
};
template<typename Real>struct matrix_inverse_sym_impl<static_matrix<4,4,Real>,static_matrix<4,4,Real> >{
static inline bool call(const static_matrix<4,4,Real>&in,static_matrix<4,4,Real>&out,Real*det)
{
const Real&i0=in[0],&i1=in[1],&i2=in[2],&i3=in[3],
&i5=in[5],&i6=in[6],&i7=in[7],
&i10=in[10],&i11=in[11],
&i15=in[15];
Real A0=i0*i5-i1*i1;
Real A1=i0*i6-i2*i1;
Real A2=i0*i7-i3*i1;
Real A3=i1*i6-i2*i5;
Real A4=i1*i7-i3*i5;
Real A5=i2*i7-i3*i6;
Real B0=i2*i7-i6*i3;
Real B1=i2*i11-i10*i3;
Real B2=i2*i15-i11*i3;
Real B3=i6*i11-i10*i7;
Real B4=i6*i15-i11*i7;
Real B5=i10*i15-i11*i11;
Real d=A0*B5-A1*B4+A2*B3+A3*B2-A4*B1+A5*B0;
if(det!=NULL)*det=d;
if(d!=0)
{
Real invD=1/d;
out[0]=(i5*B5-i6*B4+i7*B3)*invD;
out[1]=out[4]=(-i1*B5+i6*B2-i7*B1)*invD;
out[2]=out[8]=(i1*B4-i5*B2+i7*B0)*invD;
out[3]=out[12]=(-i1*B3+i5*B1-i6*B0)*invD;
out[5]=(i0*B5-i2*B2+i3*B1)*invD;
out[6]=out[9]=(-i0*B4+i1*B2-i3*B0)*invD;
out[7]=out[13]=(i0*B3-i1*B1+i2*B0)*invD;
out[10]=(i3*A4-i7*A2+i15*A0)*invD;
out[11]=out[14]=(-i3*A3+i7*A1-i11*A0)*invD;
out[15]=(i2*A3-i6*A1+i10*A0)*invD;
return true;
}
return false;
}
};
template<typename MI,typename MO>
inline bool matrix_inverse(const MI&in,MO&out,typename MI::real_type*det=NULL){
assert(in.rows==in.cols);
assert(out.rows==out.cols);
return matrix_inverse_impl<MI,MO>::call(in,out,det);
}
template<typename MI>
inline MI inverse(const MI&in){
MI out;
resize_if_dynamic(out,in.rows,in.cols);
matrix_inverse_impl<MI,MI>::call(in,out,NULL);
return out;
}
template<typename MI,typename MO>
inline bool matrix_inverse_sym(const MI&in,MO&out,typename MI::real_type*det=NULL){
assert(in.rows==in.cols);
assert(out.rows==out.cols);
return matrix_inverse_sym_impl<MI,MO>::call(in,out,det);
}
template<typename MI,typename MO>
inline bool matrix_inverse_diag(const MI&in,MO&out,typename MI::real_type*det=NULL){
assert(in.rows==in.cols);
assert(out.rows==out.cols);
assert(&in!=&out);
fill(out,0.0f);
for(unsigned i=0;i<in.rows;++i){
float w=in(i,i);
if(w!=0)
out(i,i)=1/w;
else return false;
}
return true;
}
template<typename MI>struct matrix_det_impl
{
static inline typename MI::real_type call(const MI&in)
{
assert(is_square(in));
typedef typename MI::real_type Real;
MI L,U;
if(!LU_decomposition(in,L,U))
throw std::runtime_error("Determinant failed");
Real d=1;
for(unsigned i=0;i<in.rows;++i)
d*=L(i,i)*U(i,i);
return d;
}
};
template<typename Real>struct matrix_det_impl<static_matrix<1,1,Real> >
{
static inline Real call(const static_matrix<1,1,Real>&in)
{
return in[0];
}
};
template<typename Real>struct matrix_det_impl<static_matrix<2,2,Real> >
{
static inline Real call(const static_matrix<2,2,Real>&in)
{
return in[0]*in[3]-in[1]*in[2];
}
};
template<typename Real>struct matrix_det_impl<static_matrix<3,3,Real> >
{
static inline Real call(const static_matrix<3,3,Real>&in)
{
Real A0=in[4]*in[8],
A1=in[3]*in[8],
A2=in[3]*in[7],
B0=in[5]*in[7],
B1=in[5]*in[6],
B2=in[4]*in[6];
Real K0=A0-B0,
K1=A1-B1,
K2=A2-B2;
return in[0]*K0-in[1]*K1+in[2]*K2;
}
};
template<typename Real>struct matrix_det_impl<static_matrix<4,4,Real> >
{
static inline Real call(const static_matrix<4,4,Real>&in)
{
Real A0=in[0]*in[5]-in[1]*in[4];
Real A1=in[0]*in[6]-in[2]*in[4];
Real A2=in[0]*in[7]-in[3]*in[4];
Real A3=in[1]*in[6]-in[2]*in[5];
Real A4=in[1]*in[7]-in[3]*in[5];
Real A5=in[2]*in[7]-in[3]*in[6];
Real B0=in[8]*in[13]-in[9]*in[12];
Real B1=in[8]*in[14]-in[10]*in[12];
Real B2=in[8]*in[15]-in[11]*in[12];
Real B3=in[9]*in[14]-in[10]*in[13];
Real B4=in[9]*in[15]-in[11]*in[13];
Real B5=in[10]*in[15]-in[11]*in[14];
return A0*B5-A1*B4+A2*B3+A3*B2-A4*B1+A5*B0;
}
};
template<typename MI>
inline typename MI::real_type determinant(const MI&in)
{
return matrix_det_impl<MI>::call(in);
}
template<typename MI>
inline typename MI::real_type det(const MI&in)
{
return matrix_det_impl<MI>::call(in);
}
}
#include<iostream>
#include<cstdio>
namespace heinlib
{
template<typename MxN,typename Nx1>
bool null_space_gaussian(const MxN&A,Nx1&x)
{
#pragma message("warning...null_space_basis has rare out-of-bounds access bug!!")
typedef typename MxN::real_type Real;
unsigned M=A.rows,N=A.cols;
resize_if_dynamic(x,N,1);
assert(size_is(x,N,1));
typename augment_columns<MxN,1>::result_type aug,rr;
resize_if_dynamic(aug,M,N+1);
resize_if_dynamic(rr,M,N+1);
aug.block_insert(0,0,A);
for(unsigned i=0;i<M;++i)
aug(i,N)=0;
++N;
gauss_jordan_elimination(aug,rr);
typename augment_columns<typename big_square<MxN>::result_type,1>::result_type B;
int mn=std::max(M,N);
resize_if_dynamic(B,mn,mn);
fill(B,0);
B.block_insert(0,0,rr);
for(int i=mn-1;i>=0;--i){
B(i,i)=1;
bool constr=false;
for(int j=i+1;j<mn;++j){
if(B(i,j)!=0){
constr=true;
break;
}
}
if(!constr)B(i,mn-1)=1;
}
back_substitution(B,x);
return true;
}
template<int Loud,class MxN>
bool null_space_basis(const MxN&A,dynamic_matrix<typename MxN::real_type>&B,typename MxN::real_type eps=0)
{
typedef typename MxN::real_type Real;
unsigned M=A.rows,N=A.cols;
MxN rr;
gauss_jordan_elimination<Loud-1>(A,rr,eps);
if(Loud>0)std::cout<<"RREF = \n"<<rr<<"\n";
typename big_square<MxN>::result_type S;
resize_if_dynamic(S,N,N);
fill(S,0);
S.block_insert(0,0,rr);
if(Loud>0)std::cout<<"S init= \n"<<S<<"\n";
unsigned c=0,basis_dim=0;
unsigned basis_pts[10];
for(unsigned r=0;r<N;++r)
{
while(c<N&&abs(S(r,c))<=eps){
basis_pts[basis_dim]=c;
++c;
++basis_dim;
}
if(c>r&&c<N){
unsigned shift=c-r;
for(unsigned n=N-1;int(n)-int(shift)>=int(r);--n)
{
for(unsigned j=0;j<N;++j)
S(n,j)=S(n-shift,j);
}
for(unsigned n=r;n<r+shift;++n)
for(unsigned j=0;j<N;++j)
S(n,j)=0;
r=c;
}
++c;
}
if(Loud>0)printf("basis dim = %d\n",basis_dim);
if(basis_dim==0)
return false;
if(Loud>0)std::cout<<"Arranged = \n"<<S<<"\n";
B.resize(N,basis_dim);
for(unsigned b=0;b<basis_dim;++b)
{
unsigned basis_pt=basis_pts[b];
for(unsigned r=0;r<basis_pt;++r)
B(r,b)=-S(r,basis_pt);
B(basis_pt,b)=1;
for(unsigned r=basis_pt+1;r<N;++r)
B(r,b)=0;
}
return true;
}
template<unsigned M,unsigned N,unsigned D,typename Real>
bool null_space_basis(const static_matrix<M,N,Real>&A,static_matrix<N,D,Real>&B,Real eps=1e-6)
{
assert(M<N);
static_matrix<M,N,Real>rr;
if(!gauss_jordan_elimination<0>(A,rr,eps))
return false;
typename big_square<static_matrix<M,N,Real> >::result_type S={0};
S.block_insert(0,0,rr);
unsigned c=0,basis_dim=0;
unsigned basis_pts[10];
for(unsigned r=0;r<N;++r)
{
while(c<N&&abs(S(r,c))<=eps){
basis_pts[basis_dim]=c;
++c;
++basis_dim;
}
if(c>r&&c<N){
unsigned shift=c-r;
for(unsigned n=N-1;int(n)-int(shift)>=int(r);--n)
{
for(unsigned j=0;j<N;++j)
S(n,j)=S(n-shift,j);
}
for(unsigned n=r;n<r+shift;++n)
for(unsigned j=0;j<N;++j)
S(n,j)=0;
r=c;
}
++c;
}
if(basis_dim!=D||basis_dim==0)
return false;
for(unsigned b=0;b<basis_dim;++b)
{
unsigned basis_pt=basis_pts[b];
for(unsigned r=0;r<basis_pt;++r)
B(r,b)=-S(r,basis_pt);
B(basis_pt,b)=1;
for(unsigned r=basis_pt+1;r<N;++r)
B(r,b)=0;
}
return true;
}
template<typename MxN,typename Nx1>
bool null_space_SVD(const MxN&A,Nx1&x,typename MxN::real_type eps=1e-20)
{
typedef typename MxN::real_type Real;
typedef typename square_result<Nx1>::result_type NxN;
unsigned M=A.rows,N=A.cols;
resize_if_dynamic(x,N,1);
assert(size_is(x,N,1));
MxN U;
resize_if_dynamic(U,M,N);
NxN V;
resize_if_dynamic(V,N,N);
NxN S;
resize_if_dynamic(S,N,N);
if(!SVD<false,true>(A,U,S,V,eps))
{
return false;
}
for(unsigned i=0;i<N;++i)
x[i]=V(i,N-1);
return true;
}
template<unsigned M,unsigned N,unsigned D,typename Real>
bool null_space_basis_SVD(const static_matrix<M,N,Real>&A,static_matrix<N,D,Real>&B,Real eps=1e-6)
{
static_matrix<M,N,Real>U;
static_matrix<N,N,Real>V,S;
if(!SVD<false,true>(A,U,S,V,eps))
{
return false;
}
for(unsigned i=0;i<D;++i)
B.block_insert(0,i,V.get_column(N-1-i));
return true;
}
namespace detail
{
template<typename T>
__forceinline T det_2x2(const T&m0,const T&m1,
const T&m2,const T&m3)
{
return m0*m3-m1*m2;
}
template<typename T>
__forceinline T det_3x3(const T&m0,const T&m1,const T&m2,
const T&m3,const T&m4,const T&m5,
const T&m6,const T&m7,const T&m8)
{
T A0=m4*m8,
A1=m3*m8,
A2=m3*m7,
B0=m5*m7,
B1=m5*m6,
B2=m4*m6;
T K0=A0-B0,
K1=A1-B1,
K2=A2-B2;
return m0*K0-m1*K1+m2*K2;
}
}
template<typename MxN,typename Nx1>
bool null_space_2x3(const MxN&A,Nx1&x)
{
typedef typename MxN::real_type Real;
assert(size_is(A,2,3)||size_is(A,3,3));
resize_if_dynamic(x,3,1);
assert(size_is(x,3,1));
using namespace detail;
x[0]=det_2x2(A(0,1),A(0,2),
A(1,1),A(1,2));
x[1]=-det_2x2(A(0,0),A(0,2),
A(1,0),A(1,2));
x[2]=det_2x2(A(0,0),A(0,1),
A(1,0),A(1,1));
Real norm=length_sq(x);
if(size_is(A,3,3))
{
Nx1 x2;
x2[0]=det_2x2(A(1,1),A(1,2),
A(2,1),A(2,2));
x2[1]=-det_2x2(A(1,0),A(1,2),
A(2,0),A(2,2));
x2[2]=det_2x2(A(1,0),A(1,1),
A(2,0),A(2,1));
Real norm2=length_sq(x2);
if(norm2>norm)
{
x=x2;
norm=norm2;
}
}
if(norm>0)x/=sqrt(norm);
return true;
}
template<typename MxN,typename Nx1>
bool null_space_3x4(const MxN&A,Nx1&x)
{
assert(size_is(A,3,4));
resize_if_dynamic(x,4,1);
assert(size_is(x,4,1));
using namespace detail;
x[0]=det_3x3(A(0,1),A(0,2),A(0,3),
A(1,1),A(1,2),A(1,3),
A(2,1),A(2,2),A(2,3));
x[1]=-det_3x3(A(0,0),A(0,2),A(0,3),
A(1,0),A(1,2),A(1,3),
A(2,0),A(2,2),A(2,3));
x[2]=det_3x3(A(0,0),A(0,1),A(0,3),
A(1,0),A(1,1),A(1,3),
A(2,0),A(2,1),A(2,3));
x[3]=-det_3x3(A(0,0),A(0,1),A(0,2),
A(1,0),A(1,1),A(1,2),
A(2,0),A(2,1),A(2,2));
return true;
}
template<typename MxN,typename Nx1>
bool null_space(const MxN&A,Nx1&x)
{
if(size_is(A,2,3))
return null_space_2x3(A,x);
else if(size_is(A,3,4))
return null_space_3x4(A,x);
else return null_space_SVD(A,x);
}
}
#include<string>
#include<sstream>
namespace heinlib
{
template<typename Real>
__forceinline static_matrix<2,1,Real>make_vector2(const Real&x,const Real&y)
{
static_matrix<2,1,Real>ret={x,y};
return ret;
}
template<typename Real>
__forceinline static_matrix<3,1,Real>make_vector3(Real x,Real y,Real z)
{
static_matrix<3,1,Real>ret={x,y,z};
return ret;
}
template<typename Real>
__forceinline static_matrix<4,1,Real>make_vector4(const Real&x,const Real&y,const Real&z,const Real&w)
{
static_matrix<4,1,Real>ret={x,y,z,w};
return ret;
}
template<typename T,unsigned M>
__forceinline static_matrix<M,1,T>homogeneous_divide(const static_matrix<M,1,T>&v)
{
if(v[M-1]==1)return v;
else return v/v[M-1];
}
template<unsigned M,unsigned M2,typename T>
__forceinline static_matrix<M+M2,1,T>append(const static_matrix<M,1,T>&v1,const static_matrix<M2,1,T>&v2)
{
static_matrix<M+M2,1,T>res;
for(unsigned i=0;i<M;++i)
res[i]=v1[i];
for(unsigned i=0;i<M2;++i)
res[M+i]=v2[i];
return res;
}
template<unsigned N,unsigned M,typename T>
__forceinline static_matrix<M+N,1,T>append(const static_matrix<M,1,T>&v1,const T&v2)
{
static_matrix<M+N,1,T>res;
unsigned i=0;
while(i<M){
res[i]=v1[i];
++i;
}
while(i<M+N){
res[i]=v2;
++i;
}
return res;
}
template<unsigned S,unsigned M,typename T>
__forceinline static_matrix<S,1,T>resize(const static_matrix<M,1,T>&v1)
{
static_matrix<S,1,T>res={0};
for(unsigned i=0,last=std::min(S,M);i<last;++i)
res[i]=v1[i];
return res;
}
template<unsigned S,typename T>
__forceinline dynamic_matrix<T>resize(const dynamic_matrix<T>&v1)
{
dynamic_matrix<T>res(S);
for(unsigned i=0,last=std::min(S,v1.rows*v1.cols);i<last;++i)
res[i]=v1[i];
return res;
}
template<typename T,unsigned M>
__forceinline static_matrix<M-1,1,T>realize(const static_matrix<M,1,T>&v)
{
return resize<M-1>(v/v[M-1]);
}
template<typename T,unsigned M>
__forceinline static_matrix<M+1,1,T>homogenize(const static_matrix<M,1,T>&v)
{
return append<1>(v,T(1));
}
template<typename Vector,typename Func>
__forceinline Vector binary_element_map(const Vector&a,const Vector&b,Func f)
{
assert(same_dim(a,b));
Vector out;
resize_if_dynamic(out,a.rows,1);
for(unsigned i=0;i<a.rows;++i)
out[i]=f(a[i],b[i]);
return out;
}
template<typename Vector,typename Func>
__forceinline Vector unary_element_map(const Vector&a,Func f)
{
Vector out;
resize_if_dynamic(out,a.rows,1);
for(unsigned i=0;i<a.rows;++i)
out[i]=f(a[i]);
return out;
}
template<typename Vector>
__forceinline Vector normalize(const Vector&m)
{
return m/length(m);
}
template<typename Real>
__forceinline static_matrix<3,3,Real>cross_product_matrix(const static_matrix<3,1,Real>&v)
{
static_matrix<3,3,Real>sk={0,-v.z(),v.y(),
v.z(),0,-v.x(),
-v.y(),v.x(),0};
return sk;
}
template<typename Vector>
__forceinline Vector lerp(const Vector&left,const Vector&right,typename Vector::real_type time)
{
return left*(1-time)+right*time;
}
template<typename Real>
__forceinline static_matrix<2,1,Real>rotate(const static_matrix<2,1,Real>&vec,Real rad)
{
Real cos_rad=cos(rad),sin_rad=sin(rad);
static_matrix<2,1,Real>out={cos_rad*vec.x()+sin_rad*vec.y(),-sin_rad*vec.x()+cos_rad*vec.y()};
return out;
}
template<typename Real>
__forceinline static_matrix<2,1,Real>rotate90(const static_matrix<2,1,Real>&vec)
{
static_matrix<2,1,Real>out={vec.y(),-vec.x()};
return out;
}
template<typename Real>
__forceinline static_matrix<3,1,Real>cross(const static_matrix<3,1,Real>&u,const static_matrix<3,1,Real>&v)
{
static_matrix<3,1,Real>out={u.y()*v.z()-u.z()*v.y(),
u.z()*v.x()-u.x()*v.z(),
u.x()*v.y()-u.y()*v.x()};
return out;
}
template<typename Real>
__forceinline Real cross(const static_matrix<2,1,Real>&u,const static_matrix<2,1,Real>&v)
{
return u.x()*v.y()-u.y()*v.x();
}
template<typename Vector>
__forceinline Vector reflect(const Vector&in,const Vector&norm)
{
return in-2*dot(in,norm)*norm;
}
template<typename Vector>
__forceinline Vector refract(const Vector&in,const Vector&norm,float IOR_in,float IOR_out)
{
float dp=dot(in,norm);
return in-dp*in+sqrt(sq(IOR_out)-sq(IOR_in)+sq(dp))*norm;
}
template<typename Vector>
__forceinline Vector project(const Vector&a,const Vector&b)
{
return b*(dot(a,b)/dot(b,b));
}
template<class Vector>
__forceinline Vector mirror(const Vector&X,
const Vector&pt,const Vector&normal)
{
assert(length(normal)==1);
return X-2*normal*dot(X-pt,normal);
}
template<class Real>
__forceinline static_matrix<3,1,Real>join_line(const static_matrix<3,1,Real>&p1,const static_matrix<3,1,Real>&p2)
{
return cross(p1,p2);
}
template<class Real>
__forceinline static_matrix<3,1,Real>join_line(const static_matrix<2,1,Real>&p1,const static_matrix<2,1,Real>&p2)
{
return cross(homogenize(p1),homogenize(p2));
}
template<class Real>
__forceinline static_matrix<3,1,Real>join_line(Real x1,Real y1,Real x2,Real y2)
{
return cross(make_vector3<Real>(x1,y1,1),make_vector3<Real>(x2,y2,1));
}
template<class Real>
static_matrix<3,1,Real>intersect(const static_matrix<3,1,Real>&line1,const static_matrix<3,1,Real>&line2)
{
return cross(line1,line2);
}
template<class Real>
bool intersect(
const static_matrix<2,1,Real>&s1,const static_matrix<2,1,Real>&e1,
const static_matrix<2,1,Real>&s2,const static_matrix<2,1,Real>&e2,
static_matrix<2,1,Real>*x=NULL)
{
static_matrix<2,2,Real>A={s1.x()-e1.x(),e2.x()-s2.x(),s1.y()-e1.y(),e2.y()-s2.y()};
static_matrix<2,1,Real>b={e2.x()-e1.x(),e2.y()-e1.y()};
static_matrix<2,1,Real>time;
if(!gaussian_elimination(A,b,time))
return false;
Real t1=time[0],t2=time[1];
if(t1>0&&t2>0&&t1<1&&t2<1)
{
if(x)*x=s1*t1+(1-t1)*e1;
return true;
}else return false;
}
template<class Real>
bool fuzzy_intersect(
const static_matrix<2,1,Real>&s1,const static_matrix<2,1,Real>&e1,
const static_matrix<2,1,Real>&s2,const static_matrix<2,1,Real>&e2,
Real dist_tol=0,
static_matrix<2,1,Real>*x=NULL)
{
static_matrix<2,1,Real>dir1=normalize(e1-s1),dir2=normalize(e2-s2);
static_matrix<2,2,Real>A={dir1.x(),-dir2.x(),dir1.y(),-dir2.y()};
static_matrix<2,1,Real>b={s2.x()-s1.x(),s2.y()-s1.y()};
static_matrix<2,1,Real>time;
if(!gaussian_elimination(A,b,time))
return false;
Real t1=time[0],t2=time[1];
if(t1>-dist_tol&&
t2>-dist_tol&&
t1<distance(s1,e1)+dist_tol&&
t2<distance(s2,e2)+dist_tol)
{
if(x)*x=s1+t1*dir1;
return true;
}else return false;
}
template<class Real>
static_matrix<3,1,Real>intersect(
const static_matrix<3,1,Real>&p0,
const static_matrix<3,1,Real>&p1,
const static_matrix<4,1,Real>&plane,
Real*intersect_time=NULL)
{
static_matrix<3,1,Real>dir=normalize(p1-p0);
static_matrix<3,1,float>point,normal;
point_normal_form(point,normal,plane);
Real t=(dot(point,normal)-dot(p0,normal))/dot(dir,normal);
if(intersect_time)*intersect_time=t;
return p0+t*dir;
}
template<class Real>
bool intersect(
static_matrix<3,1,Real>&pt,
static_matrix<3,1,Real>&dir,
const static_matrix<4,1,Real>&plane1,
const static_matrix<4,1,Real>&plane2)
{
static_matrix<3,1,float>n1=resize<3>(plane1),n2=resize<3>(plane2);
dir=cross(n1,n2);
Real len=length(dir);
if(len==0)return false;
dir/=len;
static_matrix<2,4,float>A;
A.block_insert(0,0,transpose(plane1));
A.block_insert(1,0,transpose(plane2));
static_matrix<4,1,float>x;
if(!null_space_SVD(A,x))
return false;
pt=realize(x);
return true;
}
template<unsigned D,class Real>
__forceinline Real point_line_dist(
const static_matrix<D,1,Real>&p,
const static_matrix<D,1,Real>&p0,
const static_matrix<D,1,Real>&p1)
{
static_matrix<D,1,Real>uL=normalize(p1-p0),w=p-p0;
return length(w-dot(w,uL)*uL);
}
template<unsigned D,class Real>
__forceinline static_matrix<D,1,Real>closest_point_on_line(
const static_matrix<D,1,Real>&p,
const static_matrix<D,1,Real>&p0,
const static_matrix<D,1,Real>&p1)
{
static_matrix<D,1,Real>uL=normalize(p1-p0),w=p-p0;
return p0+dot(w,uL)*uL;
}
template<unsigned D,class Real>
__forceinline Real sq_point_line_dist(
const static_matrix<D,1,Real>&p,
const static_matrix<D,1,Real>&p0,
const static_matrix<D,1,Real>&p1)
{
static_matrix<D,1,Real>uL=normalize(p1-p0),w=p-p0;
return length_sq(w-dot(w,uL)*uL);
}
template<unsigned D,class Real>
__forceinline Real sq_point_hyperplane_dist(
const static_matrix<D,1,Real>&x,
const static_matrix<D,1,Real>&p,
const static_matrix<D,1,Real>&n)
{
Real len=length(n);
return sq(abs(dot(x-p,n))/len);
}
template<unsigned D,class Real>
__forceinline static_matrix<D,1,Real>closest_pt_on_hyperplane(
const static_matrix<D,1,Real>&x,
const static_matrix<D,1,Real>&p,
const static_matrix<D,1,Real>&n)
{
return x-(dot(x-p,n)/dot(n,n))*n;
}
template<class Real>
__forceinline Real point_line_dist(const static_matrix<3,1,Real>&pt,const static_matrix<3,1,Real>&line)
{
return abs(dot(homogeneous_divide(pt),line))/sqrt(sq(line.x())+sq(line.y()));
}
template<class Real>
__forceinline Real point_line_dist(const static_matrix<2,1,Real>&pt,const static_matrix<3,1,Real>&line)
{
return abs(dot(homogenize(pt),line))/sqrt(sq(line.x())+sq(line.y()));
}
template<class Real>
__forceinline Real signed_point_line_dist(const static_matrix<2,1,Real>&pt,const static_matrix<3,1,Real>&line)
{
return dot(homogenize(pt),line)/sqrt(sq(line.x())+sq(line.y()));
}
template<class Real>
__forceinline static_matrix<3,1,Real>orthogonal_shift_line(const static_matrix<3,1,Real>&line,Real shift)
{
Real a=line.x(),b=line.y(),c=line.z();
Real delta=-shift*sqrt(a*a+b*b);
static_matrix<3,1,Real>l2=line;
l2.z()+=delta;
return l2;
}
template<class Real>
std::string print_line_yint_form(const static_matrix<3,1,Real>&line)
{
Real m=-line.x()/line.y(),b=-line.z()/line.y();
std::stringstream ss;
ss<<m<<"*x + "<<b;
return ss.str();
}
template<class Real>
__forceinline Real sq_point_line_dist(const static_matrix<3,1,Real>&pt,const static_matrix<3,1,Real>&line)
{
return sq(dot(homogeneous_divide(pt),line))/(sq(line.x())+sq(line.y()));
}
template<class Real>
__forceinline Real sq_point_line_dist(const static_matrix<2,1,Real>&pt,const static_matrix<3,1,Real>&line)
{
return sq(dot(homogenize(pt),line))/(sq(line.x())+sq(line.y()));
}
template<class Real>
__forceinline static_matrix<3,1,Real>closest_point_on_line(const static_matrix<3,1,Real>&pt,const static_matrix<3,1,Real>&line)
{
static_matrix<3,1,Real>v;
v[0]=(-line.z()*pt.z()-line.y()*pt.y())*line.x()+sq(line.y())*pt.x();
v[1]=sq(line.x())*pt.y()+(-line.z()*pt.z()-line.x()*pt.x())*line.y();
v[2]=sq(line.x())*pt.z()+sq(line.y())*pt.z();
return v;
}
template<class Real>
__forceinline static_matrix<2,1,Real>closest_point_on_line(const static_matrix<2,1,Real>&pt,const static_matrix<3,1,Real>&line)
{
static_matrix<3,1,Real>v;
v[0]=(-line.z()-line.y()*pt.y())*line.x()+sq(line.y())*pt.x();
v[1]=sq(line.x())*pt.y()+(-line.z()-line.x()*pt.x())*line.y();
v[2]=sq(line.x())+sq(line.y());
return realize(v);
}
template<class Real>
__forceinline static_matrix<2,1,Real>closest_point_on_segment(
const static_matrix<2,1,Real>&pt,
const static_matrix<2,1,Real>&segStart,
const static_matrix<2,1,Real>&segEnd,
int*pttype=NULL,
Real*dist=NULL)
{
Real seg_len=distance(segStart,segEnd);
static_matrix<2,1,Real>dir=(segEnd-segStart)/seg_len,w=pt-segStart;
Real t=dot(w,dir);
if(t>0&&t<seg_len)
{
if(pttype)*pttype=0;
if(dist)*dist=distance(pt,segStart+t*dir);
return segStart+t*dir;
}else
{
Real d1=distance(pt,segStart);
Real d2=distance(pt,segEnd);
if(d1<d2)
{
if(pttype)*pttype=1;
if(dist)*dist=d1;
return segStart;
}
else
{
if(pttype)*pttype=2;
if(dist)*dist=d2;
return segEnd;
}
}
}
template<class Real>
__forceinline Real point_segment_dist(
const static_matrix<2,1,Real>&pt,
const static_matrix<2,1,Real>&segStart,
const static_matrix<2,1,Real>&segEnd)
{
return distance(pt,closest_point_on_segment(pt,segStart,segEnd));
}
template<class Real>
__forceinline Real segment_distance(
const static_matrix<2,1,Real>&startA,
const static_matrix<2,1,Real>&endA,
const static_matrix<2,1,Real>&startB,
const static_matrix<2,1,Real>&endB)
{
static_matrix<2,1,Real>test=closest_point_on_segment(startB,startA,endA);
Real min_dist_sq=distance_sq(test,startB);
test=closest_point_on_segment(endB,startA,endA);
Real dist_sq=distance_sq(test,endB);
if(dist_sq<min_dist_sq)
min_dist_sq=dist_sq;
test=closest_point_on_segment(startA,startB,endB);
dist_sq=distance_sq(test,startA);
if(dist_sq<min_dist_sq)
min_dist_sq=dist_sq;
test=closest_point_on_segment(endA,startB,endB);
dist_sq=distance_sq(test,endA);
if(dist_sq<min_dist_sq)
min_dist_sq=dist_sq;
return sqrt(min_dist_sq);
}
template<typename Real>
__forceinline void closest_point_to_origin(const static_matrix<3,1,Real>&line,static_matrix<3,1,Real>&pt)
{
pt.x()=-line.x()*line.z();
pt.y()=-line.y()*line.z();
pt.z()=sq(line.x())+sq(line.y());
}
template<typename Real>
__forceinline static_matrix<2,1,Real>closest_point_to_origin(const static_matrix<3,1,Real>&line)
{
static_matrix<3,1,Real>pt=
{
-line.x()*line.z(),
-line.y()*line.z(),
sq(line.x())+sq(line.y())
};
return realize(pt);
}
template<class Real>
__forceinline Real dist_to_origin(const static_matrix<3,1,Real>&line)
{
return length(closest_point_to_origin(line));
}
template<class Real>
__forceinline Real dist_to_point(const static_matrix<2,1,float>&point,const static_matrix<3,1,Real>&line)
{
return distance(point,closest_point_on_line(point,line));
}
template<class Real>
__forceinline Real signed_point_plane_dist(
const static_matrix<4,1,Real>&point,
const static_matrix<4,1,Real>&plane)
{
return dot(homogeneous_divide(point),plane)/
sqrt(sq(plane[0])+sq(plane[1])+sq(plane[2]));
}
template<class Real>
__forceinline Real signed_point_plane_dist(
const static_matrix<3,1,Real>&point,
const static_matrix<4,1,Real>&plane)
{
return(point[0]*plane[0]+point[1]*plane[1]+point[2]*plane[2]+plane[3])/
sqrt(sq(plane[0])+sq(plane[1])+sq(plane[2]));
}
template<class Real>
__forceinline Real point_plane_dist(
const static_matrix<3,1,Real>&point,
const static_matrix<4,1,Real>&plane)
{
return abs(signed_point_plane_dist(point,plane));
}
template<class Real>
static_matrix<4,1,Real>plane_through_point(
const static_matrix<3,1,Real>&point,
const static_matrix<3,1,Real>&normal)
{
return append<1>(normal,-dot(point,normal));
}
template<class Real>
void point_normal_form(
static_matrix<3,1,Real>&point,
static_matrix<3,1,Real>&normal,
const static_matrix<4,1,Real>&plane)
{
normal=resize<3>(plane);
point=-plane[3]*normal/length_sq(normal);
}
template<class Real>
static_matrix<4,1,Real>plane_through_points(
const static_matrix<3,1,Real>&p0,
const static_matrix<3,1,Real>&p1,
const static_matrix<3,1,Real>&p2)
{
static_matrix<3,1,Real>n=cross(p2-p0,p1-p0);
return append<1>(n,-dot(p0,n));
}
}
namespace heinlib
{
template<class Real>
void empirical_CDF(
std::vector<static_matrix<2,1,Real> >&ECDF,
std::vector<Real>&samples,
Real beginx=1,Real endx=-1)
{
bool cropped=(beginx<endx);
ECDF.clear();
if(samples.empty())return;
int n=samples.size();
std::sort(samples.begin(),samples.end());
ECDF.reserve(n);
if(cropped&&samples[0]>beginx)
ECDF.push_back(make_vector2<Real>(beginx,0));
static_matrix<2,1,Real>pt={samples[0],0};
int count=1;
int i=1;
while(i<n)
{
int st=i;
while(i<n&&samples[i]==pt.x())++i;
count+=i-st;
pt.y()=Real(count)/n;
if(!cropped||(pt.x()>beginx&&pt.x()<endx))
ECDF.push_back(pt);
if(i<n)pt.x()=samples[i];
}
if(cropped&&samples.back()<endx)
ECDF.push_back(make_vector2<Real>(endx,1));
}
template<class Real>
struct ECDF
{
std::vector<static_matrix<2,1,Real> >data;
ECDF(std::vector<Real>&samples,Real beginx=1,Real endx=-1)
{
bool cropped=(beginx<endx);
if(samples.empty())return;
int n=samples.size();
std::sort(samples.begin(),samples.end());
data.reserve(n);
if(cropped&&samples[0]>beginx)
data.push_back(make_vector2<Real>(beginx,0));
static_matrix<2,1,Real>pt={samples[0],0};
int count=1;
int i=1;
while(i<n)
{
int st=i;
while(i<n&&samples[i]==pt.x())++i;
count+=i-st;
pt.y()=Real(count)/n;
if(!cropped||(pt.x()>beginx&&pt.x()<endx))
data.push_back(pt);
if(i<n)pt.x()=samples[i];
}
if(cropped&&samples.back()<endx)
data.push_back(make_vector2<Real>(endx,1));
}
Real cdf(Real x,bool interp=false)const
{
if(x<data.front().x())
return 0;
if(x>data.back().x())
return 1;
unsigned mini=0,maxi=data.size()-1;
unsigned i=(maxi+mini)>>1;
Real v=data[i].x();
while(maxi-mini>1)
{
if(x>v)
{
mini=i;
i=(maxi+mini)>>1;
v=data[i].x();
}else{
maxi=i;
i=(maxi+mini)>>1;
v=data[i].x();
}
}
if(!interp)
{
return data[mini].y();
}else
{
Real t=(x-data[mini].x())/(data[maxi].x()-data[mini].x());
return(1-t)*data[mini].y()+t*data[maxi].y();
}
}
Real integrate_probability(Real minx,Real maxx,bool interp=false)const
{
return cdf(maxx,interp)-cdf(minx,interp);
}
Real inverse_cdf(Real y,bool interp=false)const
{
if(y<data.front().y())
return data.front().x();
if(y>data.back().y())
return data.back().x();
unsigned mini=0,maxi=data.size()-1;
unsigned i=(maxi+mini)>>1;
Real v=data[i].y();
while(maxi-mini>1)
{
if(y>v)
{
mini=i;
i=(maxi+mini)>>1;
v=data[i].y();
}else{
maxi=i;
i=(maxi+mini)>>1;
v=data[i].y();
}
}
if(!interp)
{
return data[mini].x();
}else
{
Real t=(y-data[mini].y())/(data[maxi].y()-data[mini].y());
return(1-t)*data[mini].x()+t*data[maxi].x();
}
}
};
}
#pragma once
#include<vector>
#include<cassert>
#include<stdexcept>
namespace heinlib
{
namespace distributions
{
template<class Real>
struct generic_distribution
{
virtual generic_distribution*clone()const
{
throw std::runtime_error("Did not implement clone for this distribution!");
}
virtual void estimate(const std::vector<Real>&samples)
{
throw std::runtime_error("Did not implement estimation for this distribution!");
}
virtual void weighted_estimate(const std::vector<Real>&samples,const std::vector<Real>&weights)
{
throw std::runtime_error("Did not implement weighted estimation for this distribution!");
}
virtual Real sample()const
{
throw std::runtime_error("Did not implement sampling for this distribution!");
}
virtual Real pdf(Real x)const
{
throw std::runtime_error("Did not implement pdf for this distribution!");
}
virtual Real cdf(Real x)const
{
throw std::runtime_error("Did not implement cdf for this distribution!");
}
virtual Real get_mean()const
{
throw std::runtime_error("Did not implement mean value for this distribution!");
}
virtual Real inverse_cdf(Real p)const
{
throw std::runtime_error("Did not implement inverse-cdf for this distribution!");
}
};
}
}
namespace heinlib
{
namespace detail
{
void mMultiply(double*A,double*B,double*C,int m)
{
int i,j,k;double s;
for(i=0;i<m;i++)for(j=0;j<m;j++)
{s=0.;for(k=0;k<m;k++)s+=A[i*m+k]*B[k*m+j];C[i*m+j]=s;}
}
void mPower(double*A,int eA,double*V,int*eV,int m,int n)
{
double*B;int eB,i;
if(n==1){for(i=0;i<m*m;i++)V[i]=A[i];*eV=eA;return;}
mPower(A,eA,V,eV,m,n/2);
B=(double*)malloc((m*m)*sizeof(double));
mMultiply(V,V,B,m);eB=2*(*eV);
if(n%2==0){for(i=0;i<m*m;i++)V[i]=B[i];*eV=eB;}
else{mMultiply(A,B,V,m);*eV=eA+eB;}
if(V[(m/2)*m+(m/2)]>1e140){for(i=0;i<m*m;i++)V[i]=V[i]*1e-140;*eV+=140;}
free(B);
}
double Kolmogorov_distribution(int n,double d)
{
int k,m,i,j,g,eH,eQ;
double h,s,*H,*Q;
s=d*d*n;if(s>7.24||(s>3.76&&n>99))return 1-2*exp(-(2.000071+.331/sqrt(n)+1.409/n)*s);
k=(int)(n*d)+1;m=2*k-1;h=k-n*d;
H=(double*)malloc((m*m)*sizeof(double));
Q=(double*)malloc((m*m)*sizeof(double));
for(i=0;i<m;i++)for(j=0;j<m;j++)
if(i-j+1<0)H[i*m+j]=0;else H[i*m+j]=1;
for(i=0;i<m;i++){H[i*m]-=pow(h,i+1);H[(m-1)*m+i]-=pow(h,(m-i));}
H[(m-1)*m]+=(2*h-1>0?pow(2*h-1,m):0);
for(i=0;i<m;i++)for(j=0;j<m;j++)
if(i-j+1>0)for(g=1;g<=i-j+1;g++)H[i*m+j]/=g;
eH=0;mPower(H,eH,Q,&eQ,m,n);
s=Q[(k-1)*m+k-1];
for(i=1;i<=n;i++){s=s*i/n;if(s<1e-140){s*=1e140;eQ-=140;}}
s*=pow(10.,eQ);free(H);free(Q);return s;
}
}
template<class Real,class T>
Real KS_statistic(std::vector<Real>&samples,const distributions::generic_distribution<T>&distrib)
{
std::vector<static_matrix<2,1,Real> >ECDF;
empirical_CDF(ECDF,samples);
Real diff=0;
for(unsigned i=0;i<ECDF.size();++i)
diff=std::max(diff,abs(ECDF[i].y()-distrib.cdf(ECDF[i].x())));
return diff;
}
template<class Real>
Real KS_statistic(std::vector<Real>&samples1,std::vector<Real>&samples2)
{
ECDF<Real>cdf1(samples1),cdf2(samples2);
Real diff=0;
for(unsigned i=0;i<cdf1.data.size();++i)
diff=std::max<Real>(diff,abs(cdf1.data[i].y()-cdf2.cdf(cdf1.data[i].x())));
return diff;
}
template<class Real,class T>
Real KS_test(std::vector<Real>&samples,const distributions::generic_distribution<T>&distrib)
{
Real diff=KS_statistic(samples,distrib);
return 1-detail::Kolmogorov_distribution(samples.size(),diff);
}
template<class Real>
Real KS_test(std::vector<Real>&samples,std::vector<Real>&samples2)
{
Real diff=KS_statistic(samples,samples2);
return 1-detail::Kolmogorov_distribution(samples.size(),diff);
}
template<class Real>
struct twosample_KS_functor
{
Real operator()(const std::vector<Real>&samplesX,const std::vector<Real>&samplesY)const
{
std::vector<Real>*p_samplesX=const_cast<std::vector<Real>*>(&samplesX);
std::vector<Real>*p_samplesY=const_cast<std::vector<Real>*>(&samplesY);
return KS_statistic(*p_samplesX,*p_samplesY);
}
};
}
#pragma once
#include<vector>
#include<algorithm>
#pragma once
#include<vector>
#include<limits>
#pragma once
#pragma once
#if!defined(WIN32)&&!defined(__forceinline)
#define __forceinline __attribute__((always_inline))
#endif
namespace heinlib
{
template<class T>
struct remove_pointer
{
typedef T type;
__forceinline static const type&strip(const T&x)
{
return x;
}
__forceinline static type&strip(T&x)
{
return x;
}
};
template<class T>
struct remove_pointer<const T*>
{
typedef T type;
__forceinline static const type&strip(const T*x)
{
return*x;
}
};
template<class T>
struct remove_pointer<T*>
{
typedef T type;
__forceinline static type&strip(T*x)
{
return*x;
}
};
template<class T>
struct remove_pointer<T**>
{
typedef T type;
__forceinline static type&strip(T**x)
{
return**x;
}
};
template<class T>
struct remove_pointer<const T**>
{
typedef T type;
__forceinline static const type&strip(const T**x)
{
return**x;
}
};
template<class T>
struct remove_pointer<T***>
{
typedef T type;
__forceinline static type&strip(T***x)
{
return***x;
}
};
template<class T>
struct remove_pointer<const T***>
{
typedef T type;
__forceinline static const type&strip(const T***x)
{
return***x;
}
};
}
namespace heinlib
{
template<typename T>
struct cmp_mag
{
bool operator()(T a,T b)const
{
return(abs(remove_pointer<T>::strip(a))<abs(remove_pointer<T>::strip(b)));
}
};
template<typename T>
struct cmp_mag_equal
{
bool operator()(T a,T b)const
{
return(abs(remove_pointer<T>::strip(a))==abs(remove_pointer<T>::strip(b)));
}
};
template<typename T>
struct cmp_ptr
{
bool operator()(T a,T b)const
{
return(remove_pointer<T>::strip(a)<remove_pointer<T>::strip(b));
}
};
template<typename T>
struct cmp_ptr_equal
{
bool operator()(T a,T b)const
{
return(remove_pointer<T>::strip(a)==remove_pointer<T>::strip(b));
}
};
template<typename T>
struct cmp_ptr_dim
{
unsigned dim;
cmp_ptr_dim(unsigned _dim):dim(_dim){}
bool operator()(const T*a,const T*b)const
{
return((*a)[dim]<(*b)[dim]);
}
};
template<typename T>
struct cmp_ptr_dim_equal
{
unsigned dim;
cmp_ptr_dim_equal(unsigned _dim):dim(_dim){}
bool operator()(const T*a,const T*b)const
{
return((*a)[dim]==(*b)[dim]);
}
};
template<typename T>
struct cmp_dim
{
unsigned dim;
cmp_dim(unsigned _dim):dim(_dim){}
bool operator()(const T&a,const T&b)const
{
return(a[dim]<b[dim]);
}
};
template<typename T>
struct cmp_dim_equal
{
unsigned dim;
cmp_dim_equal(unsigned _dim):dim(_dim){}
bool operator()(const T&a,const T&b)const
{
return(a[dim]==b[dim]);
}
};
template<typename T1,typename T2>
struct cmp_pair_first
{
bool operator()(const std::pair<T1,T2>&a,const std::pair<T1,T2>&b)const
{
return(a.first<b.first);
}
};
template<typename T1,typename T2>
struct cmp_pair_first_equal
{
bool operator()(const std::pair<T1,T2>&a,const std::pair<T1,T2>&b)const
{
return(a.first==b.first);
}
};
template<typename T1,typename T2>
struct cmp_pair_second
{
bool operator()(const std::pair<T1,T2>&a,const std::pair<T1,T2>&b)const
{
return(a.second<b.second);
}
};
template<typename T1,typename T2>
struct cmp_pair_second_equal
{
bool operator()(const std::pair<T1,T2>&a,const std::pair<T1,T2>&b)const
{
return(a.second==b.second);
}
};
template<class Matrix>
struct cmp_index
{
unsigned index;
cmp_index(unsigned index):index(index){}
bool operator()(const Matrix&a,const Matrix&b)const
{
return(a[index]<b[index]);
}
};
}
#pragma once
#include<cassert>
namespace heinlib
{
template<typename Iter>
__forceinline typename remove_pointer<typename Iter::value_type>::type
&iter_val(const Iter&it)
{
return remove_pointer<typename Iter::value_type>::strip(*it);
}
template<typename Iter>
__forceinline const typename remove_pointer<typename Iter::value_type>::type
&const_iter_val(const Iter&it)
{
return remove_pointer<typename Iter::value_type>::strip(*it);
}
template<typename Iter,class EnableConstIter=void>
class iter_container
{
STATIC_ASSERT((is_random_access_iter<Iter>::value),MUST_BE_RANDOM_ACCESS_ITER);
const Iter&begin,&end;
const unsigned _size;
public:
typedef typename remove_pointer<typename Iter::value_type>::type value_type;
iter_container(const Iter&begin,const Iter&end):begin(begin),end(end),_size(end-begin){}
__forceinline value_type&operator[](unsigned i)const
{
assert(i<_size);
return iter_val(begin+i);
}
__forceinline unsigned size()const{return _size;};
};
template<typename Iter>
class iter_container<Iter,typename enable_if<is_const_iter<Iter> >::type>
{
STATIC_ASSERT((is_random_access_iter<Iter>::value),MUST_BE_RANDOM_ACCESS_ITER);
const Iter&begin,&end;
const unsigned _size;
public:
typedef typename remove_pointer<typename Iter::value_type>::type value_type;
iter_container(const Iter&begin,const Iter&end):begin(begin),end(end),_size(end-begin){}
__forceinline const value_type&operator[](unsigned i)const
{
assert(i<_size);
return const_iter_val(begin+i);
}
__forceinline unsigned size()const{return _size;};
};
}
#pragma once
#include<vector>
#include<cstdio>
#pragma once
#include<cstdlib>
#include<cmath>
#include<limits>
#pragma once
#ifdef WIN32
#include"windows.h"
#undef min
#undef max
#endif
namespace heinlib
{
int threadsafe_rand(int&seed)
{
static const int IA=16807,
IM=2147483647,
IQ=127773,
IR=2836,
MASK=123459876;
long k;
seed^=MASK;
k=seed/IQ;
seed=IA*(seed-k*IQ)-IR*k;
if(seed<0)seed+=IM;
int ans=seed;
seed^=MASK;
return ans;
}
template<class Real,class Enable=void>
struct random
{
static Real sign()
{
return(::rand()%2)?Real(1):Real(-1);
}
static Real uniform()
{
return::rand()/Real(RAND_MAX);
}
static Real uniform(Real minv,Real maxv)
{
return Real(::rand())/(Real(RAND_MAX))*(maxv-minv)+minv;
}
};
template<typename Integral>
struct random<Integral,typename enable_if<is_integral<Integral> >::type>
{
static Integral sign()
{
return(::rand()%2)?Integral(1):Integral(-1);
}
static Integral uniform()
{
return::rand();
}
static Integral uniform(Integral minv,Integral maxv)
{
assert((1+maxv-minv)<RAND_MAX);
return::rand()%(1+maxv-minv)+minv;
}
};
}
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
#ifndef M_SQRT2
#define M_SQRT2 1.4142135623730950488016887
#endif
#ifndef M_SQRT2PI
#define M_SQRT2PI 2.50662827463100
#endif
#ifndef M_1_SQRTPI
#define M_1_SQRTPI 0.56418958354775628694807945156077258584405062932900L
#endif
namespace heinlib
{
template<class Real>
__forceinline Real random_normal()
{
Real x,y,r;
do{
x=random<Real>::uniform(-1,1);
y=random<Real>::uniform(-1,1);
r=x*x+y*y;
}while(r>1||r==0);
return y*sqrt(-2*ln(r)/r);
};
template<class Real>
__forceinline Real stdnormal_pdf(Real u)
{
return(Real)(exp(-u*u/2)/M_SQRT2PI);
};
#define LEVMAX 10
template<class Real>
__forceinline Real quad8_stdnormal_pdf(Real a,Real b,Real Q=1.0)
{
const int w[9]={3956,23552,-3712,41984,-18160,41984,-3712,23552,
3956};
const int dw=14175;
static int level=-1;
static Real tol=1e-30;
register Real h,Q1=0.0,Q2=0.0;
register int i;
level++;
h=(b-a)/16.0;
for(i=0;i<9;i++){
Q1+=h*w[i]*stdnormal_pdf(a+i*h)/dw;
Q2+=h*w[i]*stdnormal_pdf(a+(i+8)*h)/dw;
};
if(fabs(Q1+Q2-Q)>tol*fabs(Q1+Q2)&&level<=LEVMAX){
tol=tol/2;
Q1=quad8_stdnormal_pdf(a,(a+b)/2,Q1);
Q2=quad8_stdnormal_pdf((a+b)/2,b,Q2);
tol=tol*2;
}
level--;
return Q1+Q2;
}
template<class Real>
__forceinline Real stdnormal_cdf(Real u)
{
const Real a[5]={
1.161110663653770e-002,3.951404679838207e-001,2.846603853776254e+001,
1.887426188426510e+002,3.209377589138469e+003
};
const Real b[5]={
1.767766952966369e-001,8.344316438579620e+000,1.725514762600375e+002,
1.813893686502485e+003,8.044716608901563e+003
};
const Real c[9]={
2.15311535474403846e-8,5.64188496988670089e-1,8.88314979438837594e00,
6.61191906371416295e01,2.98635138197400131e02,8.81952221241769090e02,
1.71204761263407058e03,2.05107837782607147e03,1.23033935479799725E03
};
const Real d[9]={
1.00000000000000000e00,1.57449261107098347e01,1.17693950891312499e02,
5.37181101862009858e02,1.62138957456669019e03,3.29079923573345963e03,
4.36261909014324716e03,3.43936767414372164e03,1.23033935480374942e03
};
const Real p[6]={
1.63153871373020978e-2,3.05326634961232344e-1,3.60344899949804439e-1,
1.25781726111229246e-1,1.60837851487422766e-2,6.58749161529837803e-4
};
const Real q[6]={
1.00000000000000000e00,2.56852019228982242e00,1.87295284992346047e00,
5.27905102951428412e-1,6.05183413124413191e-2,2.33520497626869185e-3
};
register Real y,z;
y=fabs(u);
if(y<=0.46875*M_SQRT2){
z=y*y;
y=u*((((a[0]*z+a[1])*z+a[2])*z+a[3])*z+a[4])
/((((b[0]*z+b[1])*z+b[2])*z+b[3])*z+b[4]);
return 0.5+y;
}
z=exp(-y*y/2)/2;
if(y<=4.0){
y=y/M_SQRT2;
y=
((((((((c[0]*y+c[1])*y+c[2])*y+c[3])*y+c[4])*y+c[5])*y+c[6])*y+c[7])*y+c[8])
/((((((((d[0]*y+d[1])*y+d[2])*y+d[3])*y+d[4])*y+d[5])*y+d[6])*y+d[7])*y+d[8]);
y=z*y;
}else{
z=z*M_SQRT2/y;
y=2/(y*y);
y=y*(((((p[0]*y+p[1])*y+p[2])*y+p[3])*y+p[4])*y+p[5])
/(((((q[0]*y+q[1])*y+q[2])*y+q[3])*y+q[4])*y+q[5]);
y=z*(M_1_SQRTPI-y);
}
return(u<0.0?y:1-y);
};
template<class Real>
__forceinline Real probit(Real p)
{
const Real a[6]={
-3.969683028665376e+01,2.209460984245205e+02,
-2.759285104469687e+02,1.383577518672690e+02,
-3.066479806614716e+01,2.506628277459239e+00
};
const Real b[5]={
-5.447609879822406e+01,1.615858368580409e+02,
-1.556989798598866e+02,6.680131188771972e+01,
-1.328068155288572e+01
};
const Real c[6]={
-7.784894002430293e-03,-3.223964580411365e-01,
-2.400758277161838e+00,-2.549732539343734e+00,
4.374664141464968e+00,2.938163982698783e+00
};
const Real d[4]={
7.784695709041462e-03,3.224671290700398e-01,
2.445134137142996e+00,3.754408661907416e+00
};
register Real q,t,u;
if(p==0.0)
return-std::numeric_limits<Real>::infinity();
if(p==1.0)
return std::numeric_limits<Real>::infinity();
q=std::min(p,1-p);
if(q>0.02425){
u=q-0.5;
t=u*u;
u=u*(((((a[0]*t+a[1])*t+a[2])*t+a[3])*t+a[4])*t+a[5])
/(((((b[0]*t+b[1])*t+b[2])*t+b[3])*t+b[4])*t+1);
}else{
t=sqrt(-2*log(q));
u=(((((c[0]*t+c[1])*t+c[2])*t+c[3])*t+c[4])*t+c[5])
/((((d[0]*t+d[1])*t+d[2])*t+d[3])*t+1);
}
t=stdnormal_cdf(u)-q;
t=t*M_SQRT2PI*exp(u*u/2);
u=u-t/(1+u*t/2);
return(p>0.5?-u:u);
};
}
namespace heinlib
{
template<class Real>
Real vector_lerp(const std::vector<Real>&samples,Real i)
{
Real i0=floor(i);
Real i1=ceil(i);
Real v0=samples[(int)i0];
Real v1=samples[(int)i1];
return v0+(i-i0)*(v1-v0)/(i1-i0);
}
template<class Real>
void quartiles(Real&median,Real&first_quartile,Real&third_quartile,std::vector<Real>&samples)
{
unsigned N=samples.size();
if(N==0){
return;
}
std::sort(samples.begin(),samples.end());
Real*mv=&samples[N>>1];
median=(N%2)?*mv:(*mv+*(mv-1))/2;
first_quartile=((N-1)%4)?vector_lerp(samples,(N-1)*Real(0.25f)):samples[N>>2];
third_quartile=((3*(N-1))%4)?vector_lerp(samples,(N-1)*Real(0.75f)):samples[(N*3)>>2];
}
template<class Real>
void robust_stats(Real&median,Real&IQR,std::vector<Real>&samples)
{
unsigned N=samples.size();
if(N==0){
median=std::numeric_limits<Real>::signaling_NaN();
IQR=std::numeric_limits<Real>::signaling_NaN();
return;
}
std::sort(samples.begin(),samples.end());
Real*mv=&samples[N>>1];
median=(N%2)?*mv:(*mv+*(mv-1))/2;
Real first_quartile=((N-1)%4)?vector_lerp(samples,(N-1)*Real(0.25f)):samples[N>>2];
Real third_quartile=((3*(N-1))%4)?vector_lerp(samples,(N-1)*Real(0.75f)):samples[(N*3)>>2];
IQR=third_quartile-first_quartile;
}
template<class Real>
void min_max(Real&minval,Real&maxval,const std::vector<Real>&samples)
{
minval=samples[0];
maxval=samples[0];
for(unsigned i=1;i<samples.size();++i)
{
if(samples[i]<minval)minval=samples[i];
if(samples[i]>maxval)maxval=samples[i];
}
}
template<class Real>
void med_min_max(Real&median,Real&minval,Real&maxval,std::vector<Real>&samples)
{
unsigned N=samples.size();
if(N==0){
median=std::numeric_limits<Real>::signaling_NaN();
minval=std::numeric_limits<Real>::signaling_NaN();
maxval=std::numeric_limits<Real>::signaling_NaN();
return;
}
std::sort(samples.begin(),samples.end());
Real*mv=&samples[N>>1];
median=(N%2)?*mv:(*mv+*(mv-1))/2;
minval=samples.front();
maxval=samples.back();
}
template<class Real>
void sample_median(Real&median,std::vector<Real>&samples)
{
if(samples.empty()){
median=std::numeric_limits<Real>::signaling_NaN();
return;
}
std::sort(samples.begin(),samples.end());
unsigned mid=(unsigned)samples.size()/2;
if(even(samples.size()))
{
median=(samples[mid]+samples[mid-1])/2;
}else{
median=samples[mid];
}
}
template<class Real>
Real median(std::vector<Real>&samples)
{
Real m;
sample_median(m,samples);
return m;
}
template<class Real>
void median_absolute_deviation(Real&median,Real&mad,std::vector<Real>&samples)
{
sample_median(median,samples);
std::vector<Real>devs(samples.size());
for(unsigned i=0;i<samples.size();++i)
devs[i]=abs(samples[i]-median);
sample_median(mad,devs);
}
template<class Real>
Real MAD(std::vector<Real>&samples)
{
Real med=median(samples);
std::vector<Real>devs(samples.size());
for(unsigned i=0;i<samples.size();++i)
devs[i]=abs(samples[i]-med);
return median(devs);
}
template<class Real>
Real weighted_percentile(std::vector<std::pair<Real,Real> >&samples,Real p)
{
std::sort(samples.begin(),samples.end(),cmp_pair_first<Real,Real>());
Real hwsum=0;
for(unsigned i=0;i<samples.size();++i)
hwsum+=samples[i].second;
hwsum*=p;
Real sum2=0;
for(unsigned i=0;i<samples.size();++i)
{
sum2+=samples[i].second;
if(sum2>=hwsum)
return samples[i].first;
}
return samples.back().first;
}
template<class Real>
Real weighted_median(std::vector<std::pair<Real,Real> >&samples)
{
return weighted_percentile(samples,(Real)0.5);
}
template<class Real>
Real WMAD(std::vector<std::pair<Real,Real> >&samples)
{
Real wm=weighted_median(samples);
std::vector<std::pair<Real,Real> >samples2=samples;
for(unsigned i=0;i<samples.size();++i)
samples2[i].first=abs(samples2[i].first-wm);
return weighted_median(samples2);
}
template<class Real,class Iter>
void sample_mean_var(Real&mean,Real&var,const Iter&begin,const Iter&end)
{
iter_container<Iter>samples(begin,end);
unsigned n=samples.size();
mean=0;
for(unsigned i=0;i<n;++i)
mean+=samples[i];
mean/=n;
var=0;
for(unsigned i=0;i<n;++i)
var+=sq(samples[i]-mean);
var/=n;
}
template<class Real>
Real variance(const std::vector<Real>&samples)
{
unsigned n=samples.size();
Real mean=0;
for(unsigned i=0;i<n;++i)
mean+=samples[i];
mean/=n;
Real var=0;
for(unsigned i=0;i<n;++i)
var+=sq(samples[i]-mean);
var/=n;
return var;
}
template<class Real,class Iter>
Real variance(const Iter&begin,const Iter&end)
{
iter_container<Iter>samples(begin,end);
Real m=0;
unsigned n=samples.size();
for(unsigned i=0;i<n;++i)
m+=samples[i];
m/=n;
Real var=0;
for(unsigned i=0;i<n;++i)
var+=sq(samples[i]-mean);
var/=n;
return var;
}
template<class Real,class Iter>
Real mean(const Iter&begin,const Iter&end)
{
Real m=0;
iter_container<Iter>samples(begin,end);
unsigned n=samples.size();
m=0;
for(unsigned i=0;i<n;++i)
m+=samples[i];
m/=n;
return m;
}
template<class Real>
Real mean(const std::vector<Real>&samples)
{
Real m={};
unsigned n=samples.size();
m=0;
for(unsigned i=0;i<n;++i)
m+=samples[i];
m/=n;
return m;
}
template<class Real>
void sample_central_moments(Real&mean,Real&m2,Real&m3,Real&m4,
const std::vector<Real>&samples)
{
unsigned n=samples.size();
mean=0;
for(unsigned i=0;i<n;++i)
mean+=samples[i];
mean/=n;
m2=0;m3=0;m4=0;
for(unsigned i=0;i<n;++i)
{
Real x=samples[i]-mean;
Real x2=x*x;
m2+=x2;
m3+=x2*x;
m4+=x2*x2;
}
m2/=n;m3/=n;m4/=n;
}
template<class Real>
void robust_central_moments(Real&m1,Real&m2,Real&m3,Real&m4,
std::vector<Real>&samples)
{
unsigned n=samples.size();
m1=median(samples);
std::vector<Real>m2s(n),m3s(n),m4s(n);
for(unsigned i=0;i<n;++i)
{
Real x=samples[i]-m1;
Real x2=x*x;
m2s[i]=x2;
m3s[i]=x2*x;
m4s[i]=x2*x2;
}
m2=median(m2s)/sq(probit(0.75));
m3=median(m3s);
m4=median(m4s)*14.5185;
}
template<class Real>
void k_statistics(Real&k1,Real&k2,Real&k3,Real&k4,
const std::vector<Real>&samples)
{
Real mean,m2,m3,m4;
sample_central_moments(mean,m2,m3,m4,samples);
Real n(samples.size());
k1=mean;
k2=n*m2/(n-1);
k3=n*n*m3/((n-1)*(n-2));
k4=n*n*((n+1)*m4-3*(n-1)*m2*m2)/((n-1)*(n-2)*(n-3));
printf("k4 is broken!!!\n");
}
template<class Real>
void sample_skewness_and_kurtosis(Real&mean,Real&skewness,Real&kurtosis,
const std::vector<Real>&samples)
{
Real mu2,mu3,mu4;
sample_central_moments(mean,mu2,mu3,mu4,samples);
skewness=mu3/pow(mu2,Real(1.5));
kurtosis=mu4/(mu2*mu2);
}
template<class Real>
void unbiased_skewness_and_kurtosis(Real&mean,Real&skewness,Real&kurtosis,
const std::vector<Real>&samples)
{
Real k2,k3,k4;
k_statistics(mean,k2,k3,k4,samples);
skewness=k3/pow(k2,Real(1.5));
kurtosis=k4/(k2*k2);
}
}
#pragma once
namespace heinlib
{
template<unsigned Dim,typename T>
struct AABB
{
static_matrix<Dim,1,T>bounds[2];
bool not_empty;
const static_matrix<Dim,1,T>&min_corner()const{return bounds[0];}
static_matrix<Dim,1,T>&min_corner(){return bounds[0];}
const static_matrix<Dim,1,T>&max_corner()const{return bounds[1];}
static_matrix<Dim,1,T>&max_corner(){return bounds[1];}
T width()const
{
return bounds[1].x()-bounds[0].x();
}
T height()const
{
return bounds[1].y()-bounds[0].y();
}
inline void init()
{
not_empty=false;
}
template<class T2>
inline void expand(const static_matrix<Dim,1,T2>&pt)
{
if(not_empty)
{
for(unsigned i=0;i<Dim;++i)
{
if(pt[i]<bounds[0][i])bounds[0][i]=pt[i];
if(pt[i]>bounds[1][i])bounds[1][i]=pt[i];
}
}else
{
bounds[0]=pt;
bounds[1]=pt;
not_empty=true;
}
}
void pad(T padding)
{
for(unsigned i=0;i<Dim;++i)
{
bounds[0][i]-=padding;
bounds[1][i]+=padding;
}
}
inline bool contains(const static_matrix<Dim,1,T>&pt)const
{
for(unsigned i=0;i<Dim;++i)
if(pt[i]<bounds[0][i]||pt[i]>bounds[1][i])return false;
return true;
}
inline void box_union(const AABB&box)
{
for(unsigned i=0;i<Dim;++i)
{
bounds[0][i]=std::min(bounds[0][i],box.bounds[0][i]);
bounds[1][i]=std::max(bounds[1][i],box.bounds[1][i]);
}
}
inline void box_intersect(const AABB&box)
{
for(unsigned i=0;i<Dim;++i)
{
bounds[0][i]=std::max(bounds[0][i],box.bounds[0][i]);
bounds[1][i]=std::min(bounds[1][i],box.bounds[1][i]);
}
}
bool intersects(const AABB&box)const
{
for(unsigned i=0;i<Dim;++i)
{
if(!(bounds[1][i]>box.bounds[0][i]&&bounds[0][i]<box.bounds[1][i]))
return false;
}
return true;
}
static_matrix<Dim,1,T>center()const
{
return(bounds[1]+bounds[0])*0.5;
}
inline T volume()const
{
T vol=(bounds[1][0]-bounds[0][0]);
for(unsigned i=1;i<Dim;++i)
vol*=(bounds[1][i]-bounds[0][i]);
return vol;
}
inline T surface_area()const
{
if(Dim==3)
{
T length=(bounds[1][0]-bounds[0][0]),
width=(bounds[1][1]-bounds[0][1]),
height=(bounds[1][2]-bounds[0][2]);
return 2*(length*width+width*height+length*height);
}else if(Dim==2)
{
return volume();
}
}
};
template<unsigned Dim,typename T>
std::ostream&operator<<(std::ostream&os,const AABB<Dim,T>&box)
{
os<<box.bounds[0]<<" to "<<box.bounds[1];
return os;
}
}
#pragma once
#include<vector>
namespace heinlib
{
template<class Real>
bool point_in_convex_polygon(const static_matrix<2,1,Real>&p,const std::vector<static_matrix<2,1,Real> >&poly)
{
Real pc=cross(poly[1]-poly[0],p-poly[0]);
for(unsigned i=1;i<poly.size();++i)
{
Real cc=cross(poly[(i+1)%poly.size()]-poly[i],p-poly[i]);
if(sign2(pc)*sign2(cc)==-1)return false;
pc=cc;
}
return true;
}
template<class Real>
bool point_in_polygon(const static_matrix<2,1,Real>&p,const std::vector<static_matrix<2,1,Real> >&poly)
{
int nvert=poly.size();
bool c=false;
for(int i=0,j=nvert-1;i<nvert;j=i++)
{
if(((poly[i].y()>p.y())!=(poly[j].y()>p.y()))&&
(p.x()<(poly[j].x()-poly[i].x())*(p.y()-poly[i].y())/(poly[j].y()-poly[i].y())+poly[i].x()))
c=!c;
}
return c;
}
template<class Real>
bool poly_in_polygon(const std::vector<static_matrix<2,1,Real> >&poly1,const std::vector<static_matrix<2,1,Real> >&poly2)
{
for(unsigned i=0;i<poly1.size();++i)
if(!point_in_polygon(poly1[i],poly2))return false;
return true;
}
}
#pragma once
#include<vector>
namespace heinlib
{
template<class T>
T&back(std::vector<T>&vec,unsigned i=0)
{
return vec[vec.size()-(i+1)];
}
template<class T>
const T&back(const std::vector<T>&vec,unsigned i=0)
{
return vec[vec.size()-(i+1)];
}
template<class T>
void append(std::vector<T>&a,const std::vector<T>&b)
{
unsigned a_size=a.size();
a.resize(a_size+b.size());
for(unsigned i=a_size,j=0;j<b.size();++i,++j)
a[i]=b[j];
}
template<class T>
void remove_duplicates(std::vector<T>&vec)
{
std::sort(vec.begin(),vec.end());
vec.erase(std::unique(vec.begin(),vec.end()),vec.end());
}
template<class T,class LessFunctor,class EqualFunctor>
void remove_duplicates(std::vector<T>&vec,const LessFunctor&cmp_less,const EqualFunctor&cmp_equal)
{
std::sort(vec.begin(),vec.end(),cmp_less);
vec.erase(std::unique(vec.begin(),vec.end(),cmp_equal),vec.end());
}
template<class T>
void print_repeats(const std::vector<T>&data)
{
unsigned count=1;
for(unsigned i=1;i<data.size();++i)
{
if(data[i]==data[i-1])
++count;
else
{
std::cout<<data[i-1];
if(count>1)
std::cout<<"("<<count<<")";
std::cout<<" ";
count=1;
}
}
std::cout<<data.back();
if(count>1)
std::cout<<"("<<count<<")";
std::cout<<" ";
}
template<class T>
float lerp_sample(const std::vector<T>&vec,float x)
{
float s=0;
if(x<0)return s;
unsigned x_max=(unsigned)ceil(x);
if(x_max>=vec.size())return s;
unsigned x_min=(unsigned)floor(x);
float t=x-x_min;
return vec[x_min]*(1-t)+vec[x_max]*t;
}
template<class T>
void stretch(std::vector<T>&vec,unsigned new_size)
{
if(new_size==vec.size())
return;
float scale=new_size/(float)vec.size();
float inv_scale=1/scale;
std::vector<float>vec2(new_size);
for(unsigned i2=0;i2<new_size;++i2)
{
float i=i2*inv_scale;
vec2[i2]=lerp_sample(vec,i);
}
std::swap(vec,vec2);
}
template<class T>
void minmax_value(T&minval,T&maxval,const std::vector<T>&vec)
{
minval=maxval=vec[0];
for(unsigned i=1;i<vec.size();++i)
{
T v=vec[i];
if(v<minval)minval=v;
if(v>maxval)maxval=v;
}
}
}
#pragma once
#include<cassert>
#include<iterator>
#include<string>
#pragma once
#include<cassert>
#if!defined(WIN32)&&!defined(__forceinline)
#define __forceinline __attribute__((always_inline))
#endif
namespace heinlib
{
template<typename T>
class unsafe_stack
{
T*data;
unsigned m_size;
public:
__forceinline unsafe_stack(unsigned Dim):m_size(0)
{
data=new T[Dim];
}
__forceinline~unsafe_stack()
{
delete[]data;
}
__forceinline unsigned size()const
{
return m_size;
}
__forceinline void clear()
{
m_size=0;
}
__forceinline void push(const T&x)
{
data[m_size++]=x;
}
__forceinline bool empty()const
{
return(m_size==0);
}
__forceinline T&top()
{
assert(m_size>0);
return data[m_size-1];
}
__forceinline const T&top()const
{
assert(m_size>0);
return data[m_size-1];
}
__forceinline void pop()
{
assert(m_size>0);
--m_size;
}
};
}
namespace heinlib
{
template<class T,class Real=float>
class priority_queue
{
struct node
{
node*left,*right;
unsigned level;
std::pair<T,Real>entry;
void delete_children()
{
if(left)left->delete_children();
delete left;
if(right)right->delete_children();
delete right;
}
void print(std::ostream&os,const std::string&prefix)const
{
os<<prefix<<" + ("<<entry.first<<","<<entry.second<<") : "<<level<<"\n";
if(left||right)
{
if(left)left->print(os,prefix+"   |");
else os<<prefix<<"   |"<<" + NULL\n";
if(right)right->print(os,prefix+"    ");
else os<<prefix<<"    "<<" + NULL\n";
}
}
};
unsigned count;
node*root;
node*_insert(const std::pair<T,Real>&entry,node*t)
{
if(t==NULL)
{
t=new node();
t->entry=entry;
t->level=0;
t->left=NULL;
t->right=NULL;
}else if(entry.second<t->entry.second)
{
t->left=_insert(entry,t->left);
}else
{
t->right=_insert(entry,t->right);
}
return split(skew(t));
}
node*skew(node*t)
{
assert(t!=NULL);
if(t->left&&t->left->level==t->level)
{
node*L=t->left;
t->left=L->right;
L->right=t;
return L;
}else return t;
}
node*split(node*t)
{
assert(t!=NULL);
if(t->right&&
t->right->right&&
t->level==t->right->right->level)
{
node*R=t->right;
t->right=R->left;
R->left=t;
++R->level;
return R;
}else return t;
}
public:
priority_queue():root(NULL),count(0)
{
}
~priority_queue()
{
clear();
}
void clear()
{
if(root){
root->delete_children();
delete root;
}
root=NULL;
count=0;
}
void insert(const T&entry,Real priority)
{
insert(std::make_pair(entry,priority));
}
void insert(const std::pair<T,Real>&entry)
{
root=_insert(entry,root);
++count;
}
unsigned size()const
{
return count;
}
bool empty()const
{
return(count==0);
}
void remove_maximum()
{
if(root==NULL)return;
node*t=root,*parent=NULL;
while(t->right)
{
parent=t;
t=t->right;
}
assert(t->right==NULL);
node*L=t->left;
delete t;
if(parent)
{
parent->right=L;
}else{
root=L;
}
--count;
}
void remove_minimum()
{
if(root==NULL)return;
node*t=root,*parent=NULL;
while(t->left)
{
parent=t;
t=t->left;
}
assert(t->left==NULL);
node*R=t->right;
delete t;
if(parent)
{
parent->left=R;
}else{
root=R;
}
--count;
}
const std::pair<T,Real>&maximum()const
{
assert(root!=NULL);
node*t=root;
while(t->right!=NULL)
t=t->right;
return t->entry;
}
const std::pair<T,Real>&minimum()const
{
assert(root!=NULL);
node*t=root;
while(t->left!=NULL)
t=t->left;
return t->entry;
}
const std::pair<T,Real>&operator[](unsigned i)const
{
const_smallest_iterator it=begin_smallest();
while(it.index!=i)++it;
return*it;
}
void print(std::ostream&os)const
{
if(root==NULL)os<<"NULL";
else root->print(os,std::string());
}
struct const_smallest_iterator:std::iterator<std::forward_iterator_tag,void,void,void,void>
{
heinlib::unsafe_stack<const node*>stk;
const node*t;
unsigned index;
const_smallest_iterator(const priority_queue*tree)
:index(0),stk(tree->size())
{
if(tree->size()>0)
{
t=tree->root;
while(t->left){
stk.push(t);
t=t->left;
}
}else
{
t=NULL;
return;
}
}
const_smallest_iterator()
:t(NULL),stk(1)
{
}
const std::pair<T,Real>&operator*()const{
return t->entry;
}
const_smallest_iterator&operator++()
{
if(!t)return*this;
if(t->right)
{
t=t->right;
while(t->left){
stk.push(t);
t=t->left;
}
}else if(!stk.empty())
{
t=stk.top();
stk.pop();
}
else t=NULL;
++index;
return*this;
}
bool operator==(const const_smallest_iterator&it)const
{
if(t==NULL)return(it.t==NULL);
else if(it.t==NULL)return(t==NULL);
return(t==it.t);
}
bool operator!=(const const_smallest_iterator&it)const
{
return!(*this==it);
}
const_smallest_iterator operator++(int)
{
const_smallest_iterator copy=*this;++*this;return copy;
}
};
const_smallest_iterator begin_smallest()const
{
return const_smallest_iterator(this);
}
const_smallest_iterator end_smallest()const
{
return const_smallest_iterator();
}
struct const_largest_iterator:std::iterator<std::forward_iterator_tag,void,void,void,void>
{
heinlib::unsafe_stack<const node*>stk;
const node*t;
unsigned index;
const_largest_iterator(const priority_queue*tree)
:index(0),stk(tree->size())
{
if(tree->size()>0)
{
t=tree->root;
while(t->right){
stk.push(t);
t=t->right;
}
}else
{
t=NULL;
return;
}
}
const_largest_iterator()
:t(NULL),stk(1)
{
}
const std::pair<T,Real>&operator*()const{
return t->entry;
}
const_largest_iterator&operator++()
{
if(!t)return*this;
if(t->left)
{
t=t->left;
while(t->right){
stk.push(t);
t=t->right;
}
}else if(!stk.empty())
{
t=stk.top();
stk.pop();
}
else t=NULL;
++index;
return*this;
}
bool operator==(const const_largest_iterator&it)const
{
if(t==NULL)return(it.t==NULL);
else if(it.t==NULL)return(t==NULL);
return(t==it.t);
}
bool operator!=(const const_largest_iterator&it)const
{
return!(*this==it);
}
const_largest_iterator operator++(int)
{
const_largest_iterator copy=*this;++*this;return copy;
}
};
const_largest_iterator begin_largest()const
{
return const_largest_iterator(this);
}
const_largest_iterator end_largest()const
{
return const_largest_iterator();
}
};
template<class T,class Real>
std::ostream&operator<<(std::ostream&os,const priority_queue<T,Real>&tree)
{
tree.print(os);
return os;
}
}
#include<iostream>
namespace heinlib
{
template<unsigned Dim,class Real>
class kd_tree
{
public:
typedef static_matrix<Dim,1,Real>Point;
typedef std::pair<Point,unsigned>Exemplar;
private:
struct node
{
Point min_corner,max_corner;
unsigned split_dim;
Real split_value;
node*lower,*upper;
std::vector<const Exemplar*>leaves;
Point closest_point(const Point&p)const
{
Point closest;
for(unsigned i=0;i<Dim;++i)
{
if(p[i]<=min_corner[i])
closest[i]=min_corner[i];
else if(p[i]>=max_corner[i])
closest[i]=max_corner[i];
else closest[i]=p[i];
}
return closest;
}
Real sq_dist_to_point(const Point&p)const
{
return distance_sq(p,closest_point(p));
}
node()
:lower(NULL),upper(NULL),leaves(NULL)
{
for(unsigned i=0;i<Dim;++i){
min_corner[i]=std::numeric_limits<Real>::infinity();
max_corner[i]=-std::numeric_limits<Real>::infinity();
}
}
~node()
{
delete lower;
delete upper;
}
};
struct region
{
const Exemplar*ex;
node*n;
region():ex(NULL),n(NULL){}
region(node*n):ex(NULL),n(n){}
region(const Exemplar*ex):ex(ex),n(NULL){}
};
node*root;
unsigned _size;
public:
struct cmp_exemplars_index
{
bool operator()(const Exemplar*a,const Exemplar*b)const
{
return a->second<b->second;
}
};
struct cmp_exemplars_index_equal
{
bool operator()(const Exemplar*a,const Exemplar*b)const
{
return a->second==b->second;
}
};
kd_tree():root(NULL),_size(0){}
~kd_tree()
{
delete root;
}
unsigned size()const
{
return _size;
}
bool empty()const
{
return(_size==0);
}
template<class Iter>
kd_tree(const Iter&begin,const Iter&end,unsigned Leaf=3):root(NULL)
{
if(!build(begin,end,Leaf))
throw std::runtime_error("build kd_tree failed (probably empty list)");
}
template<class Iter>
bool build(const Iter&begin,const Iter&end,unsigned Leaf=3)
{
if(root)delete root;
root=new node();
return _build(*root,begin,end,Leaf);
}
const Exemplar*nearest_neighbor(const Point&p,Real*sq_dist=NULL)const
{
if(!root||empty())return NULL;
Real min_sq_dist=std::numeric_limits<Real>::infinity();
const Exemplar*neighbor;
_find_nearest_neighbor(root,p,neighbor,min_sq_dist);
if(sq_dist)*sq_dist=min_sq_dist;
return neighbor;
}
unsigned neighbors(std::vector<const Exemplar*>&neighbors,const Point&p,const Real&radius_sq)const
{
if(!root||empty())return NULL;
_find_neighbors(neighbors,root,p,radius_sq);
return(unsigned)neighbors.size();
}
void k_nearest_neighbors(std::vector<const Exemplar*>&neighbors,const Point&p,unsigned k)const
{
if(!root||empty())return;
priority_queue<region,float>queue;
queue.insert(region(root),root->sq_dist_to_point(p));
while(!queue.empty()&&neighbors.size()<k)
{
region br=queue.minimum().first;
queue.remove_minimum();
if(br.ex)
{
neighbors.push_back(br.ex);
}else
{
if(br.n->leaves.empty())
{
queue.insert(region(br.n->lower),br.n->lower->sq_dist_to_point(p));
queue.insert(region(br.n->upper),br.n->upper->sq_dist_to_point(p));
}else
{
for(unsigned i=0;i<br.n->leaves.size();++i)
{
const Exemplar*ex=br.n->leaves[i];
queue.insert(region(ex),distance_sq(ex->first,p));
}
}
}
}
}
unsigned line_segment_neighbors(std::vector<const Exemplar*>&neighbors,const Point&pA,const Point&pB,const Real&radius_sq)const
{
if(!root||empty())return 0;
Real dist=sqrt(radius_sq);
Point dir=pB-pA;
Real seg_length=length(dir);
dir/=seg_length;
for(Real t=0;t<seg_length;t+=dist)
{
Point p=pA+dir*t;
_find_neighbors(neighbors,root,p,radius_sq);
}
remove_duplicates(neighbors,cmp_exemplars_index(),cmp_exemplars_index_equal());
return neighbors.size();
}
template<bool Enable>
unsigned points_in_convex_polygon(std::vector<const Exemplar*>&points,const std::vector<Point>&poly)const
{
STATIC_ASSERT((Dim==2),DIM_MUST_BE_2_FOR_THIS_FUNC);
if(!root||empty())return 0;
AABB<2,Real>box;
box.init();
for(unsigned i=0;i<poly.size();++i)
box.expand(poly[i]);
Point center=box.center();
Real radius_sq=0;
for(unsigned i=0;i<poly.size();++i)
radius_sq=std::max(radius_sq,distance_sq(center,poly[i]));
std::vector<const Exemplar*>radius_neighbors;
neighbors(radius_neighbors,center,radius_sq);
points.reserve(points.size()+radius_neighbors.size());
for(unsigned i=0;i<radius_neighbors.size();++i)
{
if(point_in_convex_polygon(radius_neighbors[i]->first,poly))
points.push_back(radius_neighbors[i]);
}
return(unsigned)points.size();
}
private:
void _find_neighbors(std::vector<const Exemplar*>&neighbors,const node*start,const Point&p,const Real&radius_sq)const
{
if(!root||empty())return;
if(start->sq_dist_to_point(p)>radius_sq)
return;
if(!start->leaves.empty())
{
for(typename std::vector<const Exemplar*>::const_iterator it=start->leaves.begin();
it!=start->leaves.end();++it)
{
Real dist=distance_sq(p,(*it)->first);
if(dist<radius_sq)
neighbors.push_back(*it);
}
}else
{
if(p[start->split_dim]<start->split_value)
{
_find_neighbors(neighbors,start->lower,p,radius_sq);
_find_neighbors(neighbors,start->upper,p,radius_sq);
}else{
_find_neighbors(neighbors,start->upper,p,radius_sq);
_find_neighbors(neighbors,start->lower,p,radius_sq);
}
}
}
void _find_nearest_neighbor(
const node*start,
const Point&p,
Exemplar const*&neighbor,
Real&min_sq_dist)const
{
if(!root||empty())return;
if(min_sq_dist<std::numeric_limits<Real>::infinity()&&
start->sq_dist_to_point(p)>=min_sq_dist)
return;
if(!start->leaves.empty())
{
for(typename std::vector<const Exemplar*>::const_iterator it=start->leaves.begin();
it!=start->leaves.end();++it)
{
Real dist=distance_sq(p,(**it).first);
if(dist<min_sq_dist){
min_sq_dist=dist;
neighbor=*it;
}
}
}else
{
if(p[start->split_dim]<start->split_value){
_find_nearest_neighbor(start->lower,p,neighbor,min_sq_dist);
_find_nearest_neighbor(start->upper,p,neighbor,min_sq_dist);
}else{
_find_nearest_neighbor(start->upper,p,neighbor,min_sq_dist);
_find_nearest_neighbor(start->lower,p,neighbor,min_sq_dist);
}
}
}
template<class Iter>
bool _build(node&n,const Iter&begin,const Iter&end,unsigned Leaf)
{
if(begin==end)return false;
iter_container<Iter>exemplars(begin,end);
for(unsigned i=0;i<exemplars.size();++i)
{
const Point&ex=exemplars[i].first;
for(unsigned d=0;d<Dim;++d)
{
n.min_corner[d]=std::min(n.min_corner[d],ex[d]);
n.max_corner[d]=std::max(n.max_corner[d],ex[d]);
}
}
if(exemplars.size()>Leaf)
{
Real largest_spread=0;
n.split_dim=0;
for(unsigned d=0;d<Dim;++d)
{
Real spread=n.max_corner[d]-n.min_corner[d];
if(spread>largest_spread){
largest_spread=spread;
n.split_dim=d;
}
}
std::vector<Real>vals(exemplars.size());
for(unsigned i=0;i<exemplars.size();++i)
vals[i]=exemplars[i].first[n.split_dim];
n.split_value=median(vals);
std::vector<std::pair<Point,unsigned>*>lower,upper;
for(unsigned i=0;i<exemplars.size();++i)
{
if(exemplars[i].first[n.split_dim]<=n.split_value)
lower.push_back(&exemplars[i]);
else upper.push_back(&exemplars[i]);
}
if(upper.empty())
{
for(unsigned i=0;i<exemplars.size();++i)
n.leaves.push_back(&exemplars[i]);
_size+=exemplars.size();
}else{
n.lower=new node();
n.upper=new node();
if(!_build(*n.lower,lower.begin(),lower.end(),Leaf))return false;
if(!_build(*n.upper,upper.begin(),upper.end(),Leaf))return false;
}
}else if(exemplars.size()>0)
{
for(unsigned i=0;i<exemplars.size();++i)
n.leaves.push_back(&exemplars[i]);
_size+=exemplars.size();
}
return true;
}
};
}
namespace heinlib
{
namespace detail
{
template<unsigned D,class Real>
struct cmp_nearest_neighbors
{
static_matrix<D,1,Real>v;
cmp_nearest_neighbors(static_matrix<D,1,Real>v)
:v(v){}
bool operator()(const std::pair<static_matrix<3,1,Real>,unsigned>a,const std::pair<static_matrix<3,1,Real>,unsigned>b)const
{
return(distance_sq(v,a.first)<distance_sq(v,b.first));
}
};
template<class Real>
struct cmp_nearest_neighbors<1,Real>
{
Real v;
cmp_nearest_neighbors(Real v)
:v(v){}
bool operator()(const std::pair<Real,unsigned>a,const std::pair<Real,unsigned>b)const
{
return(abs(v-a.first)<abs(v-b.first));
}
};
}
template<class Real>
Real nearest_neighbor_statistic(
const std::vector<Real>&samplesX,
const std::vector<Real>&samplesY,
unsigned J=3)
{
std::vector<std::pair<static_matrix<1,1,Real>,unsigned> >pool;
pool.reserve(samplesX.size()+samplesY.size());
static_matrix<1,1,Real>vec;
for(unsigned i=0;i<samplesX.size();++i)
{
vec[0]=samplesX[i];
pool.push_back(std::make_pair(vec,0));
}
for(unsigned i=0;i<samplesY.size();++i)
{
vec[0]=samplesY[i];
pool.push_back(std::make_pair(vec,1));
}
kd_tree<1,Real>tree;
tree.build(pool.begin(),pool.end());
unsigned sum=0;
for(unsigned r=1;r<=J;++r)
{
for(unsigned i=0;i<samplesX.size();++i)
{
std::vector<const kd_tree<1,Real>::Exemplar*>neighbors;
vec[0]=samplesX[i];
tree.k_nearest_neighbors(neighbors,vec,r+1);
sum+=(neighbors.back()->second==0);
}
for(unsigned i=0;i<samplesY.size();++i)
{
std::vector<const kd_tree<1,Real>::Exemplar*>neighbors;
vec[0]=samplesY[i];
tree.k_nearest_neighbors(neighbors,vec,r+1);
sum+=(neighbors.back()->second==1);
}
}
return sum/(float)(pool.size()*J);
}
template<unsigned D,class Real>
Real nearest_neighbor_statistic(
const std::vector<static_matrix<D,1,Real> >&samplesX,
const std::vector<static_matrix<D,1,Real> >&samplesY,
unsigned J=3)
{
std::vector<std::pair<static_matrix<D,1,Real>,unsigned> >pool;
pool.reserve(samplesX.size()+samplesY.size());
for(unsigned i=0;i<samplesX.size();++i)
pool.push_back(std::make_pair(samplesX[i],0));
for(unsigned i=0;i<samplesY.size();++i)
pool.push_back(std::make_pair(samplesY[i],1));
kd_tree<D,Real>tree;
tree.build(pool.begin(),pool.end());
unsigned sum=0;
for(unsigned r=1;r<=J;++r)
{
for(unsigned i=0;i<samplesX.size();++i)
{
std::vector<const kd_tree<D,Real>::Exemplar*>neighbors;
tree.k_nearest_neighbors(neighbors,samplesX[i],r+1);
sum+=(neighbors.back()->second==0);
}
for(unsigned i=0;i<samplesY.size();++i)
{
std::vector<const kd_tree<D,Real>::Exemplar*>neighbors;
tree.k_nearest_neighbors(neighbors,samplesY[i],r+1);
sum+=(neighbors.back()->second==1);
}
}
return sum/(float)(pool.size()*J);
}
struct nearest_neighbor_functor
{
template<class Real>
Real operator()(const std::vector<Real>&samplesX,const std::vector<Real>&samplesY)const
{
return nearest_neighbor_statistic(samplesX,samplesY);
}
template<unsigned D,class Real>
Real operator()(const std::vector<static_matrix<D,1,Real> >&samplesX,
const std::vector<static_matrix<D,1,Real> >&samplesY)const
{
return nearest_neighbor_statistic(samplesX,samplesY);
}
};
}
#pragma once
namespace heinlib
{
template<class Real,unsigned D>
Real e_distance(
const std::vector<static_matrix<D,1,Real> >&samples1,
const std::vector<static_matrix<D,1,Real> >&samples2,
bool normalize=false)
{
unsigned n1=samples1.size(),n2=samples2.size();
Real sum_ab=0;
for(unsigned i=0;i<n1;++i)
for(unsigned j=0;j<n2;++j)
sum_ab+=distance(samples1[i],samples2[j]);
Real sum_aa=0;
for(unsigned i=0;i<n1;++i)
for(unsigned j=0;j<n1;++j)
sum_aa+=distance(samples1[i],samples1[j]);
Real sum_bb=0;
for(unsigned i=0;i<n2;++i)
for(unsigned j=0;j<n2;++j)
sum_bb+=distance(samples2[i],samples2[j]);
Real e=n1*n2*(2*sum_ab/(n1*n2)-sum_aa/sq(n1)-sum_bb/sq(n2))/(n1+n2);
if(normalize)return e/(n1+n2);
else return e;
}
template<class Real>
Real e_distance(
const std::vector<Real>&samples1,
const std::vector<Real>&samples2,
bool normalize=false)
{
unsigned n1=samples1.size(),n2=samples2.size();
Real sum_ab=0;
for(unsigned i=0;i<n1;++i)
for(unsigned j=0;j<n2;++j)
sum_ab+=abs(samples1[i]-samples2[j]);
Real sum_aa=0;
for(unsigned i=0;i<n1;++i)
for(unsigned j=0;j<n1;++j)
sum_aa+=abs(samples1[i]-samples1[j]);
Real sum_bb=0;
for(unsigned i=0;i<n2;++i)
for(unsigned j=0;j<n2;++j)
sum_bb+=abs(samples2[i]-samples2[j]);
Real e=n1*n2*(2*sum_ab/(n1*n2)-sum_aa/sq(n1)-sum_bb/sq(n2))/(n1+n2);
if(normalize)return e/(n1+n2);
else return e;
}
struct e_distance_functor
{
template<class Real>
Real operator()(const std::vector<Real>&samplesX,const std::vector<Real>&samplesY)const
{
return e_distance(samplesX,samplesY);
}
template<class Real,unsigned D>
Real operator()(
const std::vector<static_matrix<D,1,Real> >&samplesX,
const std::vector<static_matrix<D,1,Real> >&samplesY)const
{
return e_distance(samplesX,samplesY);
}
};
template<class Real,unsigned D>
Real fuzzy_e_distance(
const std::vector<static_matrix<D,1,Real> >&samples1,
const std::vector<Real>&weights1,
const std::vector<static_matrix<D,1,Real> >&samples2,
const std::vector<Real>&weights2,
bool normalize=false)
{
Real n1=0,n2=0;
for(unsigned i=0;i<weights1.size();++i)
n1+=weights1[i];
for(unsigned i=0;i<weights2.size();++i)
n2+=weights2[i];
Real sum_ab=0;
for(unsigned i=0;i<n1;++i)
for(unsigned j=0;j<n2;++j)
sum_ab+=weights1[i]*weights2[j]*distance(samples1[i],samples2[j]);
Real sum_aa=0;
for(unsigned i=0;i<n1;++i)
for(unsigned j=0;j<n1;++j)
sum_aa+=weights1[i]*weights1[j]*distance(samples1[i],samples1[j]);
Real sum_bb=0;
for(unsigned i=0;i<n2;++i)
for(unsigned j=0;j<n2;++j)
sum_bb+=weights2[i]*weights2[j]*distance(samples2[i],samples2[j]);
Real e=n1*n2*(2*sum_ab/(n1*n2)-sum_aa/sq(n1)-sum_bb/sq(n2))/(n1+n2);
if(normalize)return e/(n1+n2);
else return e;
}
}
#pragma once
#pragma once
#include<vector>
#include<cassert>
namespace heinlib
{
namespace distributions
{
template<class Real>
class normal:public distributions::generic_distribution<Real>
{
public:
Real mu,sigma;
normal(Real sigma=1,Real mu=0)
:mu(mu),sigma(sigma)
{
}
normal(const std::vector<Real>&samples)
{
estimate(samples);
}
generic_distribution<Real>*clone()const
{
return(generic_distribution<Real>*)(new normal(*this));
}
void estimate(const std::vector<Real>&samples)
{
unsigned n=samples.size();
mu=0;
for(unsigned i=0;i<n;++i)
mu+=samples[i];
mu/=n;
sigma=0;
for(unsigned i=0;i<n;++i)
sigma+=sq(samples[i]-mu);
sigma=sqrt(sigma/n);
}
void weighted_estimate(const std::vector<Real>&samples,const std::vector<Real>&weights)
{
unsigned n=samples.size();
mu=0;
Real wsum=0;
for(unsigned i=0;i<n;++i){
mu+=samples[i]*weights[i];
wsum+=weights[i];
}
mu/=wsum;
sigma=0;
for(unsigned i=0;i<n;++i)
sigma+=sq(samples[i]-mu)*weights[i];
sigma=sqrt(sigma/wsum);
}
__forceinline Real mean()const
{
return mu;
}
__forceinline Real sample()const
{
return mu+random_normal<Real>()*sigma;
}
__forceinline Real pdf(Real x)const
{
return stdnormal_pdf((x-mu)/sigma)/sigma;
}
__forceinline Real negative_log_likelihood(Real x)const
{
return sq((x-mu)/sigma)*0.5;
}
__forceinline Real cdf(Real x)const
{
return stdnormal_cdf((x-mu)/sigma);
}
__forceinline Real inverse_cdf(Real p)const
{
return mu+sigma*probit(p);
}
normal operator*(const normal&d)const
{
return product_normal_approx(*this,d);
}
normal operator+(const normal&d)const
{
return linear_combination(*this,d);
}
};
template<class Real>
normal<Real>linear_transform(const normal<Real>&X,Real a,Real b)
{
return normal<Real>(X.sigma*a,X.mu+b);
}
template<class Real>
normal<Real>linear_combination(const normal<Real>&X,const normal<Real>&Y,Real wX=1,Real wY=1)
{
return normal<Real>(sqrt(sq(wX*X.sigma)+sq(wY*Y.sigma)),wX*X.mu+wY*Y.mu);
}
template<class Real>
normal<Real>intersection_distribution(const normal<Real>&X,const normal<Real>&Y)
{
Real sXX=sq(X.sigma),sYY=sq(Y.sigma);
Real new_sigma=sqrt(2*sXX*sYY/(sXX+sYY));
Real new_mean=(X.mu*sYY+Y.mu*sXX)/(sXX+sYY);
return normal<Real>(new_sigma,new_mean);
}
template<class Real>
normal<Real>product_normal_approx(const normal<Real>&X,const normal<Real>&Y)
{
if(Y.sigma>X.sigma)return product_normal_approx(Y,X);
Real sigmaZ=sqrt(sq(Y.sigma)+sq(X.sigma)+sq(X.mu)+sq(Y.mu)-2*X.mu*Y.mu);
return normal<Real>(sigmaZ,Y.mu);
}
template<class Real>
normal<Real>random_field_union(const normal<Real>&X,const normal<Real>&Y,Real n=1,Real m=1)
{
Real uW=(n*X.mu+m*Y.mu)/(n+m);
Real sW=sqrt((n*(sq(X.sigma)+sq(uW-X.mu))+m*(sq(Y.sigma)+sq(uW-Y.mu)))/(n+m));
return normal<Real>(sW,uW);
}
template<class Real>
std::ostream&operator<<(std::ostream&os,const normal<Real>&distrib)
{
os<<"normal{ sigma="<<distrib.sigma<<", mu="<<distrib.mu<<" }";
return os;
}
}
}
#pragma once
#pragma once
namespace heinlib
{
template<typename MxN,typename NxM>
bool pseudo_inverse_SVD(const MxN&A,NxM&Apinv,typename MxN::real_type eps=2.2e-10)
{
typedef typename square_result<NxM>::result_type NxN;
unsigned M=A.rows,N=A.cols;
resize_if_dynamic(Apinv,N,M);
assert(size_is(Apinv,N,M));
MxN U;
resize_if_dynamic(U,M,N);
NxN S,V;
resize_if_dynamic(S,N,N);
resize_if_dynamic(V,N,N);
if(!SVD<true,true>(A,U,S,V,eps))return false;
for(unsigned i=0;i<std::min(M,N);++i)
{
if(S(i,i)!=0)
S(i,i)=1/S(i,i);
}
Apinv=V*transpose(S)*transpose(U);
return true;
}
}
#pragma once
#pragma once
namespace heinlib
{
template<class NxN>
typename enable_if<is_matrix<NxN>,bool>::type
LDL_factorization(NxN&A)
{
assert(is_square(A));
unsigned N=A.rows;
typedef typename NxN::real_type Real;
typename column_vector<NxN>::result_type v;
resize_if_dynamic(v,N,1);
assert(size_is(v,N,1));
for(unsigned j=0;j<N;++j)
{
for(unsigned i=0;i<j;++i)
v[i]=A(j,i)*A(i,i);
v[j]=A(j,j);
for(unsigned i=0;i<j;++i)
v[j]-=A(j,i)*v[i];
if(v[j]==0)return false;
A(j,j)=v[j];
for(unsigned i=j+1;i<N;++i)
{
Real sum=0;
for(unsigned k=0;k<j;++k)
sum+=A(i,k)*v[k];
A(i,j)=(A(i,j)-sum)/v[j];
}
}
return true;
}
template<class NxN>
typename enable_if<is_matrix<NxN>,bool>::type
LDL_factorization(const NxN&A,NxN&L,NxN&D)
{
unsigned N=A.rows;
assert(is_square(A));
assert(size_is(L,N,N));
assert(size_is(D,N,N));
NxN At=A;
fill(L,0);
fill(D,0);
if(!LDL_factorization(At))
return false;
for(unsigned i=0;i<N;++i){
for(unsigned j=0;j<i;++j)
L(i,j)=At(i,j);
L(i,i)=1;
D(i,i)=At(i,i);
}
return true;
}
}
namespace heinlib
{
template<typename MxM>
bool cholesky_RRt(const MxM&A,MxM&R)
{
MxM Ainv;
if(!matrix_inverse(A,Ainv))
return false;
if(!cholesky_decomposition(Ainv,R))
return false;
R=transpose(inverse(R));
return true;
}
template<typename MxM>
bool cholesky_decomposition(const MxM&A,MxM&L)
{
typedef typename MxM::real_type Real;
unsigned M=A.rows;
assert(is_square(A));
resize_if_dynamic(L,M,M);
assert(size_is(L,M,M));
for(unsigned r=0;r<M-1;++r)
for(unsigned c=M-1;c>r;--c)
L(r,c)=0;
if(A(0,0)<=0){
return false;
}
Real l00=sqrt(A(0,0));
L(0,0)=l00;
for(unsigned r=1;r<M;++r)
L(r,0)=A(r,0)/l00;
for(unsigned c=1;c<M;++c){
Real sum=0,l;
for(unsigned s=0;s<c;++s){
l=L(c,s);
sum+=l*l;
}
Real temp=A(c,c)-sum;
if(temp<=0){
return false;
}
L(c,c)=sqrt(temp);
for(unsigned r=c+1;r<M;++r){
Real sum=0;
for(unsigned s=0;s<c;++s)
sum+=L(c,s)*L(r,s);
L(r,c)=(A(r,c)-sum)/L(c,c);
}
}
return true;
}
template<typename MxM>
bool cholesky_decomposition_enforce(const MxM&A,MxM&L)
{
typedef typename MxM::real_type Real;
unsigned M=A.rows;
assert(is_square(A));
resize_if_dynamic(L,M,M);
assert(size_is(L,M,M));
MxM D;
resize_if_dynamic(D,M,M);
assert(size_is(D,M,M));
if(!LDL_factorization(A,L,D))
return false;
for(unsigned i=0;i<M;++i)
{
Real&d=D(i,i);
if(d<-1)return false;
d=(d<0)?0:sqrt(d);
}
L*=D;
return true;
}
}
#pragma once
namespace heinlib
{
template<class Nx1>
void sample_mean(Nx1&mean,const std::vector<Nx1>&samples)
{
typedef Nx1::real_type Real;
resize_if_dynamic(mean,samples[0].rows,samples[0].cols);
fill(mean,Real(0));
for(unsigned i=0;i<samples.size();++i)
mean+=samples[i];
mean/=(Real)samples.size();
}
template<class NxN,class Nx1>
void sample_covariance_matrix(NxN&covar,Nx1&mean,const std::vector<Nx1>&samples)
{
typedef NxN::real_type Real;
sample_mean(mean,samples);
resize_if_dynamic(covar,mean.rows,mean.rows);
fill(covar,Real(0));
Nx1 diff;
for(unsigned i=0;i<samples.size();++i)
{
diff=(samples[i]-mean);
covar+=diff*transpose(diff);
}
covar/=(Real)samples.size();
}
template<unsigned N,class Real>
void sample_covariance_known_mean(static_matrix<N,N,Real>&covar,
const static_matrix<N,1,Real>&mean,
const std::vector<static_matrix<N,1,Real> >&samples)
{
static_matrix<N,1,Real>diff;
for(unsigned i=0;i<samples.size();++i)
{
diff=(samples[i]-mean);
covar+=diff*transpose(diff);
}
covar/=(Real)samples.size();
}
template<unsigned Dim,class Real>
Real sq_mahalanobis_distance(const static_matrix<Dim,1,Real>&X,
const static_matrix<Dim,Dim,Real>&SigmaInv,
const static_matrix<Dim,1,Real>&Mean)
{
static_matrix<Dim,1,Real>Y=X-Mean;
return(transpose(Y)*SigmaInv*Y)[0];
}
template<unsigned Dim,class Real>
Real mahalanobis_distance(const static_matrix<Dim,1,Real>&X,
const static_matrix<Dim,Dim,Real>&SigmaInv,
const static_matrix<Dim,1,Real>&Mean)
{
static_matrix<Dim,1,Real>Y=X-Mean;
return sqrt((transpose(Y)*SigmaInv*Y)[0]);
}
}
#include<vector>
namespace heinlib
{
namespace distributions
{
template<unsigned N,class Real>
class multivariate_normal:distributions::generic_distribution<static_matrix<N,1,Real> >
{
Real mult;
public:
static_matrix<N,N,Real>L;
bool no_variance;
bool init(Real minvar=0)
{
if(minvar>0)
{
for(unsigned i=0;i<N;++i)
if(this->covar(i,i)<minvar)this->covar(i,i)=minvar;
}
if(!no_variance)
{
if(!cholesky_decomposition(covar,L))
{
fill(L,0);
no_variance=true;
}
}
if(!no_variance)
{
Real det;
if(!matrix_inverse_sym(covar,covar_inv,&det))
{
no_variance=true;
}
Real two_pi_nth=ipow<N,Real>::eval(constants<Real>::two_pi());
mult=1/sqrt(two_pi_nth*det);
}
return true;
}
static_matrix<N,1,Real>mean;
static_matrix<N,N,Real>covar,covar_inv;
multivariate_normal(Real var=1)
{
fill(mean,0);
fill(covar,0);
for(unsigned i=0;i<N;++i)
covar(i,i)=var;
no_variance=(var==0);
if(!init())throw std::exception("multivariate_normal init failed!");
}
generic_distribution*clone()const
{
return(generic_distribution*)(new multivariate_normal(*this));
}
void set_covar(const static_matrix<N,N,Real>&_covar,Real minvar=0)
{
covar=_covar;
if(!init(minvar))throw std::exception("multivariate_normal init failed!");
}
multivariate_normal(const static_matrix<N,1,Real>&mean,const static_matrix<N,N,Real>&covar,Real minvar=0)
:mean(mean),covar(covar),no_variance(false)
{
if(!init(minvar))throw std::exception("multivariate_normal init failed!");
}
multivariate_normal(const static_matrix<N,N,Real>&covar,Real minvar=0)
:covar(covar),no_variance(false)
{
fill(mean,0);
if(!init(minvar))throw std::exception("multivariate_normal init failed!");
}
multivariate_normal(const static_matrix<N,1,Real>&mean,Real var=1)
:mean(mean)
{
no_variance=(var==0);
fill(covar,0);
for(unsigned i=0;i<N;++i)
covar(i,i)=var;
if(!init())throw std::exception("multivariate_normal init failed!");
}
multivariate_normal(const std::vector<static_matrix<N,1,Real> >&samples,Real minvar=0)
{
estimate(samples,minvar);
}
void estimate(const std::vector<static_matrix<N,1,Real> >&samples,Real minvar=0)
{
no_variance=false;
sample_covariance_matrix(covar,mean,samples);
if(!init(minvar))throw std::exception("multivariate_normal init failed!");
}
void estimate(const std::vector<static_matrix<N,1,Real> >&samples,const std::vector<Real>&weights,Real minvar=0)
{
no_variance=false;
fill(mean,Real(0));
Real wsum=0;
for(unsigned i=0;i<samples.size();++i){
Real w=weights[i];
mean+=samples[i]*w;
wsum+=w;
}
mean/=wsum;
fill(covar,Real(0));
static_matrix<N,1,Real>diff;
for(unsigned i=0;i<samples.size();++i)
{
diff=(samples[i]-mean);
covar+=diff*transpose(diff)*weights[i];
}
covar/=wsum;
if(!init(minvar))throw std::exception("multivariate_normal init failed!");
}
__forceinline static_matrix<N,1,Real>sample()const
{
static_matrix<N,1,Real>X;
for(unsigned i=0;i<N;++i)
X[i]=random_normal<Real>();
return L*X+mean;
}
__forceinline void sample(static_matrix<N,1,Real>&X)const
{
for(unsigned i=0;i<N;++i)
X[i]=random_normal<Real>();
X=L*X+mean;
}
__forceinline Real pdf(const static_matrix<N,1,Real>&X)const
{
static_matrix<N,1,Real>S=X-mean;
return mult*exp(-Real(0.5)*(transpose(S)*covar_inv*S)[0]);
}
__forceinline Real negative_log_likelihood(const static_matrix<N,1,Real>&X)const
{
static_matrix<N,1,Real>S=X-mean;
return(transpose(S)*covar_inv*S)[0];
}
__forceinline Real mahalanobis_distance(const static_matrix<N,1,Real>&X)const
{
static_matrix<N,1,Real>S=X-mean;
return sqrt((transpose(S)*covar_inv*S)[0]);
}
};
template<class Real>
class dynamic_multivariate_normal:distributions::generic_distribution<dynamic_matrix<Real> >
{
dynamic_matrix<Real>L;
Real mult;
public:
unsigned N;
bool init()
{
if(mean.cols!=1||mean.rows!=covar.rows||!is_square(covar))
return false;
if(!cholesky_decomposition(covar,L))
{
std::cout<<"covar failed = \n"<<covar<<"\n";
return false;
}
Real det;
if(!matrix_inverse(covar,covar_inv,&det))
{
std::cout<<"no inverse? \n"<<covar<<"\n";
return false;
}
Real two_pi_nth=pow(constants<Real>::two_pi(),Real(N));
mult=1/sqrt(two_pi_nth*det);
return true;
}
dynamic_matrix<Real>mean,covar,covar_inv;
void set_covar(const dynamic_matrix<Real>&_covar)
{
covar=_covar;
if(!init())throw std::exception("multivariate_normal init failed!");
}
dynamic_multivariate_normal(){}
dynamic_multivariate_normal(const dynamic_matrix<Real>&mean,const dynamic_matrix<Real>&covar)
:mean(mean),covar(covar)
{
N=mean.rows;
if(!init())throw std::exception("multivariate_normal init failed!");
}
dynamic_multivariate_normal(const dynamic_matrix<Real>&mean,Real var=1)
:mean(mean)
{
N=mean.rows;
covar.resize(N,N);
fill(covar,0);
for(unsigned i=0;i<N;++i)
covar(i,i)=var;
if(!init())throw std::exception("multivariate_normal init failed!");
}
void estimate(const std::vector<dynamic_matrix<Real> >&samples)
{
N=samples[0].rows;
mean.resize(N,1);
covar.resize(N,N);
sample_covariance_matrix(covar,mean,samples);
if(!init())throw std::exception("multivariate_normal init failed!");
}
__forceinline dynamic_matrix<Real>sample()const
{
if(no_variance)
return mean;
else
{
dynamic_matrix<Real>X;
for(unsigned i=0;i<N;++i)
X[i]=random_normal<Real>();
return L*X+mean;
}
}
__forceinline void sample(dynamic_matrix<Real>&X)const
{
if(no_variance)
return mean;
else
{
for(unsigned i=0;i<N;++i)
X[i]=random_normal<Real>();
X=L*X+mean;
}
}
__forceinline Real pdf(const dynamic_matrix<Real>&X)const
{
if(no_variance)
{
if(X==mean)
return 1;
else return 0;
}else
{
dynamic_matrix<Real>S=X-mean;
return mult*exp(-Real(0.5)*(transpose(S)*covar_inv*S)[0]);
}
}
__forceinline Real sq_mahalanobis_distance(const dynamic_matrix<Real>&X)const
{
dynamic_matrix<Real>S=X-mean;
return(transpose(S)*covar_inv*S)[0];
}
};
template<unsigned N,class Real>
static_matrix<N,1,Real>argmax_mutual_probability(
const multivariate_normal<N,Real>&d1,
const multivariate_normal<N,Real>&d2)
{
static_matrix<N,1,Real>v=d1.mean-d2.mean;
Real var1=(transpose(v)*d1.covar*v)[0];
Real var2=(transpose(v)*d2.covar*v)[0];
return(var2*d1.mean+var1*d2.mean)/(var1+var2);
}
template<unsigned N,class Real>
multivariate_normal<N,Real>mutual_distribution(
const multivariate_normal<N,Real>&d1,
const multivariate_normal<N,Real>&d2)
{
static_matrix<N,1,Real>v=d1.mean-d2.mean;
Real var1=(transpose(v)*d1.covar*v)[0];
Real var2=(transpose(v)*d2.covar*v)[0];
multivariate_normal<N,Real>d(
(var2*d1.mean+var1*d2.mean)/(var1+var2),
(var2*d1.covar+var1*d2.covar)/(var1+var2));
return d;
}
template<unsigned Dim,class Real>
std::ostream&operator<<(std::ostream&os,const multivariate_normal<Dim,Real>&distrib)
{
os<<"mv_normal{ covar=\n"<<distrib.covar<<", mean="<<distrib.mean<<" }";
return os;
}
}
}
#pragma once
#pragma once
namespace heinlib
{
template<class Iter>
typename enable_if<is_floating<typename Iter::value_type>,typename Iter::value_type>::type
rule_of_thumb_bandwidth(const Iter&begin,const Iter&end)
{
typedef Iter::value_type Real;
iter_container<Iter>samples(begin,end);
Real mean,var;
sample_mean_var(mean,var,begin,end);
return 1.06*sqrt(var)/pow(samples.size(),0.2);
}
template<class Iter>
typename enable_if<is_matrix<typename Iter::value_type>,typename Iter::value_type::real_type>::type
rule_of_thumb_bandwidth(const Iter&begin,const Iter&end)
{
iter_container<Iter>samples(begin,end);
typedef Iter::value_type Vector;
typedef Vector::real_type Real;
Vector mean={};
for(unsigned i=0;i<samples.size();++i)
mean+=samples[i];
mean/=samples.size();
Real var=0;
for(unsigned i=0;i<samples.size();++i)
var+=distance_sq(samples[i],mean);
var/=samples.size();
return 1.06*sqrt(var)/pow(samples.size(),0.2);
}
}
namespace heinlib
{
template<unsigned D,class Real>
struct kernel_density
{
std::vector<std::pair<static_matrix<D,1,Real>,unsigned> >exemplars;
kd_tree<D,Real>tree;
Real sigma;
kernel_density(const std::vector<static_matrix<D,1,Real> >&points,Real _sigma=-1)
:sigma(_sigma),exemplars(points.size())
{
for(unsigned i=0;i<points.size();++i)
{
exemplars[i].first=points[i];
exemplars[i].second=i;
}
tree.build(exemplars.begin(),exemplars.end());
if(sigma<=0)
sigma=rule_of_thumb_bandwidth(points.begin(),points.end());
}
Real pdf(const static_matrix<D,1,Real>&x)const
{
Real radius_sq=sq(3*sigma);
std::vector<const kd_tree<D,Real>::Exemplar*>neighbors;
Real norm=1.0112614f/sqrt(pow(sq(sigma)*constants<Real>::two_pi(),(Real)D));
if(tree.neighbors(neighbors,x,radius_sq)>0)
{
Real wsum=0;
for(unsigned i=0;i<neighbors.size();++i)
wsum+=exp(-Real(0.5f)*sq(distance(neighbors[i]->first,x)/sigma));
return norm*wsum/exemplars.size();
}else return 0;
}
};
template<class Real>
struct kernel_density<1,Real>
{
std::vector<Real>exemplars;
Real sigma;
kernel_density(const std::vector<Real>&points,Real _sigma=-1)
:sigma(_sigma),exemplars(points)
{
if(sigma<=0)sigma=rule_of_thumb_bandwidth(points.begin(),points.end());
std::sort(exemplars.begin(),exemplars.end());
}
Real pdf(Real x)const
{
Real radius=3*sigma;
if(exemplars.front()>x+radius||exemplars.back()<x-radius)
return 0;
unsigned left=0,right=exemplars.size()-1;
unsigned starti=(left+right)>>1;
Real v=exemplars[starti];
unsigned iter=0;
while(right-left>1&&(v>x+radius||v<x-radius))
{
if(v>x+radius)
{
right=starti;
starti=(left+right)>>1;
v=exemplars[starti];
}else if(v<x-radius)
{
left=starti;
starti=(left+right)>>1;
v=exemplars[starti];
}
}
Real norm=1.0112614f/(sigma*constants<Real>::sqrt_two_pi());
Real wsum=0;
for(unsigned i=starti;i<exemplars.size()&&exemplars[i]<x+radius;++i)
wsum+=exp(-Real(0.5f)*sq((exemplars[i]-x)/sigma));
for(int i=(int)starti-1;i>0&&exemplars[i]>x-radius;--i)
wsum+=exp(-Real(0.5f)*sq((exemplars[i]-x)/sigma));
return norm*wsum/exemplars.size();
}
};
}
namespace heinlib
{
template<class Distribution,class Real>
Real intersection_union_statistic(
const std::vector<Real>&samplesX,
const std::vector<Real>&samplesY)
{
Distribution X(samplesX),Y(samplesY);
Real integrate_intersection=0,integrate_union=0;
for(unsigned i=0;i<samplesX.size();++i)
{
const Real&z=samplesX[i];
Real pX=X.pdf(z),pY=Y.pdf(z);
integrate_intersection+=min(pX,pY);
integrate_union+=max(pX,pY);
}
for(unsigned i=0;i<samplesY.size();++i)
{
const Real&z=samplesY[i];
Real pX=X.pdf(z),pY=Y.pdf(z);
integrate_intersection+=min(pX,pY);
integrate_union+=max(pX,pY);
}
return 1-integrate_intersection/integrate_union;
}
template<class Distribution,class Real,unsigned D>
Real intersection_union_statistic(
const std::vector<static_matrix<D,1,Real> >&samplesX,
const std::vector<static_matrix<D,1,Real> >&samplesY)
{
Distribution X(samplesX),Y(samplesY);
Real integrate_intersection=0,integrate_union=0;
for(unsigned i=0;i<samplesX.size();++i)
{
const static_matrix<D,1,Real>&z=samplesX[i];
Real pX=X.pdf(z),pY=Y.pdf(z);
integrate_intersection+=min(pX,pY);
integrate_union+=max(pX,pY);
}
for(unsigned i=0;i<samplesY.size();++i)
{
const static_matrix<D,1,Real>&z=samplesY[i];
Real pX=X.pdf(z),pY=Y.pdf(z);
integrate_intersection+=min(pX,pY);
integrate_union+=max(pX,pY);
}
return 1-integrate_intersection/integrate_union;
}
template<class Distribution>
struct intersection_union_functor
{
template<class Real>
Real operator()(const std::vector<Real>&samplesX,const std::vector<Real>&samplesY)const
{
return intersection_union_statistic<Distribution>(samplesX,samplesY);
}
template<unsigned D,class Real>
Real operator()(
const std::vector<static_matrix<D,1,Real> >&samplesX,
const std::vector<static_matrix<D,1,Real> >&samplesY)const
{
return intersection_union_statistic<Distribution>(samplesX,samplesY);
}
};
}
#pragma once
#include<vector>
#pragma once
#include<vector>
#include<algorithm>
namespace heinlib
{
template<class T>
void randomize(std::vector<T>&data)
{
typedef std::vector<T>::size_type size_t;
size_t size=data.size();
for(size_t i=0;i<size;++i)
{
size_t j=rand()%size;
while(i==j)j=rand()%size;
std::swap(data[i],data[j]);
}
}
}
namespace heinlib
{
template<class Real,class Functor>
Real bootstrap_pvalue(
Functor&f,
const std::vector<Real>&samplesX,
const std::vector<Real>&samplesY,
bool nullIsLower=true,
unsigned bootstrap_samples=1000)
{
unsigned total_samples=samplesX.size()+samplesY.size();
Real statistic_value=f(samplesX,samplesY);
std::vector<Real>pooled;
append(pooled,samplesX);
append(pooled,samplesY);
std::vector<Real>bs_samplesX(samplesX.size()),
bs_samplesY(samplesY.size());
unsigned more_extreme=0;
for(unsigned i=0;i<bootstrap_samples;++i)
{
randomize(pooled);
for(unsigned i=0;i<bs_samplesX.size();++i)
bs_samplesX[i]=pooled[i];
for(unsigned i=0;i<bs_samplesY.size();++i)
bs_samplesY[i]=pooled[bs_samplesX.size()+i];
Real v=f(bs_samplesX,bs_samplesY);
if(nullIsLower)
{
if(v>=statistic_value)
++more_extreme;
}else{
if(v<=statistic_value)
++more_extreme;
}
}
return more_extreme/(float)bootstrap_samples;
}
template<class Real,unsigned D,class Functor>
Real bootstrap_pvalue(
Functor&f,
const std::vector<static_matrix<D,1,Real> >&samplesX,
const std::vector<static_matrix<D,1,Real> >&samplesY,
bool nullIsLower=true,
unsigned bootstrap_samples=1000)
{
unsigned total_samples=samplesX.size()+samplesY.size();
Real statistic_value=f(samplesX,samplesY);
std::vector<static_matrix<D,1,Real> >pooled;
append(pooled,samplesX);
append(pooled,samplesY);
std::vector<static_matrix<D,1,Real> >bs_samplesX(samplesX.size()),
bs_samplesY(samplesY.size());
unsigned more_extreme=0;
for(unsigned i=0;i<bootstrap_samples;++i)
{
randomize(pooled);
for(unsigned i=0;i<bs_samplesX.size();++i)
bs_samplesX[i]=pooled[i];
for(unsigned i=0;i<bs_samplesY.size();++i)
bs_samplesY[i]=pooled[bs_samplesX.size()+i];
Real v=f(bs_samplesX,bs_samplesY);
if(nullIsLower)
{
if(v>=statistic_value)
++more_extreme;
}else{
if(v<=statistic_value)
++more_extreme;
}
}
return more_extreme/(float)bootstrap_samples;
}
}
#include<fstream>
#pragma once
#include<iostream>
#include<fstream>
#include<string>
namespace heinlib
{
class xstream
{
std::ostream&file;
public:
xstream(std::ostream&_file):file(_file){}
xstream&operator<<(const std::string&val){file<<val;std::cout<<val;return*this;}
xstream&operator<<(const char*val){file<<val;std::cout<<val;return*this;}
xstream&operator<<(bool val){file<<val;std::cout<<val;return*this;}
xstream&operator<<(short val){file<<val;std::cout<<val;return*this;}
xstream&operator<<(unsigned short val){file<<val;std::cout<<val;return*this;}
xstream&operator<<(int val){file<<val;std::cout<<val;return*this;}
xstream&operator<<(unsigned int val){file<<val;std::cout<<val;return*this;}
xstream&operator<<(long val){file<<val;std::cout<<val;return*this;}
xstream&operator<<(unsigned long val){file<<val;std::cout<<val;return*this;}
#ifdef WIN32
xstream&operator<<(__int64 val){file<<val;std::cout<<val;return*this;}
xstream&operator<<(unsigned __int64 val){file<<val;std::cout<<val;return*this;}
#endif
#ifdef __APPLE__
xstream&operator<<(__int64_t val){file<<val;std::cout<<val;return*this;}
xstream&operator<<(__uint64_t val){file<<val;std::cout<<val;return*this;}
#endif
xstream&operator<<(float val){file<<val;std::cout<<val;return*this;}
xstream&operator<<(double val){file<<val;std::cout<<val;return*this;}
xstream&operator<<(long double val){file<<val;std::cout<<val;return*this;}
xstream&operator<<(void*val){file<<val;std::cout<<val;return*this;}
xstream&operator<<(std::ostream&(*val)(std::ostream&val)){file<<val;std::cout<<val;return*this;}
xstream&operator<<(std::streambuf*val){file<<val;std::cout<<val;return*this;}
xstream&operator<<(std::ios&(*val)(std::ios&)){file<<val;std::cout<<val;return*this;}
xstream&operator<<(std::ios_base&(*val)(std::ios_base&)){file<<val;std::cout<<val;return*this;}
};
}
#pragma once
#pragma once
namespace heinlib
{
template<typename T>
struct constants;
template<typename Real>
struct constants<static_matrix<3,1,Real> >
{
static inline static_matrix<3,1,Real>e1()
{
static const static_matrix<3,1,Real>ret={1,0,0};
return ret;
}
static inline static_matrix<3,1,Real>e2()
{
static const static_matrix<3,1,Real>ret={0,1,0};
return ret;
}
static inline static_matrix<3,1,Real>e3()
{
static const static_matrix<3,1,Real>ret={0,0,1};
return ret;
}
};
template<typename Real>
struct constants<static_matrix<4,1,Real> >
{
static inline static_matrix<4,1,Real>e1()
{
static const static_matrix<4,1,Real>ret={1,0,0,0};
return ret;
}
static inline static_matrix<4,1,Real>e2()
{
static const static_matrix<4,1,Real>ret={0,1,0,0};
return ret;
}
static inline static_matrix<4,1,Real>e3()
{
static const static_matrix<4,1,Real>ret={0,0,1,0};
return ret;
}
static inline static_matrix<4,1,Real>e4()
{
static const static_matrix<4,1,Real>ret={0,0,0,1};
return ret;
}
};
template<typename Real>
struct constants<static_matrix<2,2,Real> >
{
static inline static_matrix<2,2,Real>identity()
{
static const static_matrix<2,2,Real>ret={1,0,0,1};
return ret;
}
};
template<typename Real>
struct constants<static_matrix<3,3,Real> >
{
static inline static_matrix<3,3,Real>identity()
{
static const static_matrix<3,3,Real>ret={1,0,0,0,1,0,0,0,1};
return ret;
}
};
template<typename Real>
struct constants<static_matrix<4,4,Real> >
{
static inline static_matrix<4,4,Real>identity()
{
static const static_matrix<4,4,Real>ret={1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1};
return ret;
}
};
};
#pragma once
namespace heinlib
{
template<class Real>
struct quaternion
{
Real x,y,z,w;
__forceinline quaternion operator*(const quaternion&q)const;
__forceinline quaternion operator+(const quaternion&q)const;
__forceinline quaternion operator*(const Real&s)const;
__forceinline static_matrix<3,1,Real>rotate(const static_matrix<3,1,Real>&v)const;
__forceinline static_matrix<3,1,Real>operator*(const static_matrix<3,1,Real>&v)const;
__forceinline static_matrix<3,3,Real>to_matrix()const;
__forceinline static_matrix<3,1,Real>to_axis_angle()const;
__forceinline static_matrix<3,1,Real>get_axis()const;
__forceinline Real get_angle()const;
__forceinline bool is_real()const
{
return(x==0&&y==0&&z==0);
}
__forceinline bool is_pure()const
{
return(w=0);
}
};
template<typename T>struct constants;
template<typename Real>
struct constants<quaternion<Real> >
{
static inline quaternion<Real>identity()
{
static const quaternion<Real>ret={0,0,0,1};
return ret;
}
};
template<class Real>
std::ostream&operator<<(std::ostream&os,const quaternion<Real>&q)
{
os<<"("<<q.x<<", "<<q.y<<", "<<q.z<<", "<<q.w<<")";
return os;
}
template<class Real>
__forceinline quaternion<Real>make_quaternion(Real x,Real y,Real z,Real w)
{
quaternion<Real>q={x,y,z,w};
return q;
}
template<class Real>
__forceinline Real norm_sq(const quaternion<Real>&q)
{
return sq(q.x)+sq(q.y)+sq(q.z)+sq(q.w);
}
template<class Real>
__forceinline Real norm(const quaternion<Real>&q)
{
return sqrt(norm_sq(q));
}
template<class Real>
__forceinline quaternion<Real>normalize(const quaternion<Real>&q)
{
Real len=norm(q);
quaternion<Real>r={q.x/len,q.y/len,q.z/len,q.w/len};
return r;
}
template<class Real>
__forceinline quaternion<Real>make_quaternion(const static_matrix<3,1,Real>&axis,const Real&angle)
{
Real sin_a=sin(angle/2);
Real cos_a=cos(angle/2);
quaternion<Real>q=
{
axis.x()*sin_a,
axis.y()*sin_a,
axis.z()*sin_a,
cos_a
};
return normalize(q);
}
template<class Real>
__forceinline quaternion<Real>make_quaternion(const static_matrix<3,1,Real>&axis_angle)
{
Real angle=length(axis_angle);
if(angle==0)
return constants<quaternion<Real> >::identity();
else
return make_quaternion(axis_angle/angle,angle);
}
template<class Real>
__forceinline quaternion<Real>quaternion<Real>::operator*(const quaternion<Real>&q)const
{
quaternion<Real>r=
{
q.w*x+q.x*w-q.y*z+q.z*y,
q.w*y+q.x*z+q.y*w-q.z*x,
q.w*z-q.x*y+q.y*x+q.z*w,
q.w*w-q.x*x-q.y*y-q.z*z
};
return r;
}
template<class Real>
__forceinline quaternion<Real>quaternion<Real>::operator*(const Real&s)const
{
quaternion<Real>r={x*s,y*s,z*s,w*s};
return r;
}
template<class Real>
__forceinline quaternion<Real>operator*(const Real&s,const quaternion<Real>&q)
{
quaternion<Real>r={q.x*s,q.y*s,q.z*s,q.w*s};
return r;
}
template<class Real>
__forceinline quaternion<Real>quaternion<Real>::operator+(const quaternion<Real>&q)const
{
quaternion<Real>r={x+q.x,y+q.y,z+q.z,w+q.w};
return r;
}
template<class Real>
__forceinline quaternion<Real>conjugate(const quaternion<Real>&q)
{
quaternion<Real>r={-q.x,-q.y,-q.z,q.w};
return r;
}
template<class Real>
__forceinline quaternion<Real>inverse(const quaternion<Real>&q)
{
return conjugate(normalize(q));
}
template<class Real>
__forceinline quaternion<Real>as_quaternion(const static_matrix<3,1,Real>&v)
{
quaternion<Real>q={v.x(),v.y(),v.z(),0};
return q;
}
template<class Real>
__forceinline static_matrix<3,1,Real>as_vector(const quaternion<Real>&q)
{
static_matrix<3,1,Real>v={q.x,q.y,q.z};
return v;
}
template<class Real>
__forceinline static_matrix<3,1,Real>quaternion<Real>::rotate(const static_matrix<3,1,Real>&v)const
{
quaternion<Real>q=normalize(*this);
return as_vector(q*as_quaternion(v)*conjugate(q));
}
template<class Real>
__forceinline static_matrix<3,1,Real>quaternion<Real>::operator*(const static_matrix<3,1,Real>&v)const
{
return this->rotate(v);
}
template<class Real>
__forceinline static_matrix<3,3,Real>quaternion<Real>::to_matrix()const
{
static_matrix<3,3,Real>R;
quaternion<Real>q=normalize(*this);
Real xx=q.x*q.x;
Real xy=q.x*q.y;
Real xz=q.x*q.z;
Real xw=q.x*q.w;
Real yy=q.y*q.y;
Real yz=q.y*q.z;
Real yw=q.y*q.w;
Real zz=q.z*q.z;
Real zw=q.z*q.w;
R[0]=1-2*(yy+zz);
R[1]=2*(xy-zw);
R[2]=2*(xz+yw);
R[3]=2*(xy+zw);
R[4]=1-2*(xx+zz);
R[5]=2*(yz-xw);
R[6]=2*(xz-yw);
R[7]=2*(yz+xw);
R[8]=1-2*(xx+yy);
return R;
}
template<class Real>
__forceinline static_matrix<3,1,Real>quaternion<Real>::to_axis_angle()const
{
quaternion<Real>q=normalize(*this);
Real angle=2*acos(q.w);
static_matrix<3,1,Real>axis={};
Real denom=1-sq(q.w);
if(denom==0)return axis;
Real temp=1/denom;
axis.x()=q.x*temp;
axis.y()=q.y*temp;
axis.z()=q.z*temp;
return normalize(axis)*angle;
}
template<class Real>
__forceinline static_matrix<3,1,Real>quaternion<Real>::get_axis()const
{
return normalize(to_axis_angle());
}
template<class Real>
__forceinline Real quaternion<Real>::get_angle()const
{
return length(to_axis_angle());
}
namespace detail
{
template<typename Real>
void quat_from_perfect_matrix(static_matrix<4,1,Real>&q,const static_matrix<3,3,Real>&R)
{
Real tr=trace(R);
if(tr>0)
{
Real s=Real(0.5)/heinlib::sqrt(tr+1);
q.w()=Real(0.25)/s;
q.x()=(R(2,1)-R(1,2))*s;
q.y()=(R(0,2)-R(2,0))*s;
q.z()=(R(1,0)-R(0,1))*s;
}else{
if(R(0,0)>R(1,1)&&R(0,0)>R(2,2)){
Real s=2*heinlib::sqrt(1+R(0,0)-R(1,1)-R(2,2));
q.w()=(R(2,1)-R(1,2))/s;
q.x()=Real(0.25)*s;
q.y()=(R(0,1)+R(1,0))/s;
q.z()=(R(0,2)+R(2,0))/s;
}else if(R(1,1)>R(2,2)){
Real s=2*heinlib::sqrt(1+R(1,1)-R(0,0)-R(2,2));
q.w()=(R(0,2)-R(2,0))/s;
q.x()=(R(0,1)+R(1,0))/s;
q.y()=Real(0.25)*s;
q.z()=(R(1,2)+R(2,1))/s;
}else{
Real s=2*heinlib::sqrt(1+R(2,2)-R(0,0)-R(1,1));
q.w()=(R(1,0)-R(0,1))/s;
q.x()=(R(0,2)+R(2,0))/s;
q.y()=(R(1,2)+R(2,1))/s;
q.z()=Real(0.25)*s;
}
}
return;
}
template<typename Real>
void q_method(static_matrix<4,1,Real>&q,const static_matrix<3,3,Real>&R,unsigned power_iters=5)
{
if(power_iters>0)
{
static_matrix<4,4,Real>K={R(0,0)-R(1,1)-R(2,2),R(1,0)+R(0,1),R(2,0)+R(0,2),R(1,2)-R(2,1),
K[1],R(1,1)-R(0,0)-R(2,2),R(2,1)+R(1,2),R(2,0)-R(0,2),
K[2],K[6],R(2,2)-R(0,0)-R(1,1),R(0,1)-R(1,0),
K[3],K[7],K[11],R(0,0)+R(1,1)+R(2,2)};
q.w()*=Real(-1);
for(unsigned iter=0;iter<power_iters;++iter)
q=normalize(K*q);
q.w()*=Real(-1);
}
}
}
template<class Real>
__forceinline quaternion<Real>make_quaternion(const static_matrix<3,3,Real>&R,unsigned iters=20)
{
static_matrix<4,1,Real>q;
detail::quat_from_perfect_matrix(q,R);
detail::q_method(q,R,iters);
return make_quaternion(q.x(),q.y(),q.z(),q.w());
}
template<class Real>
__forceinline Real angle(const quaternion<Real>&a,const quaternion<Real>&b)
{
quaternion<Real>na=normalize(a),nb=normalize(b);
Real dp=na.x*nb.x+na.y*nb.y+na.z*nb.z+na.w*nb.w;
return 2*acos(dp);
}
template<class Real>
Real geodesic_distance(const quaternion<Real>&a,const quaternion<Real>&b)
{
Real theta=angle(a,b);
return constants<Real>::sqrt_2()*abs(theta);
}
template<class Real>
quaternion<Real>slerp(const quaternion<Real>&a,const quaternion<Real>&b,Real t)
{
Real cosHalfTheta=a.w*b.w+a.x*b.x+a.y*b.y+a.z*b.z;
if(abs(cosHalfTheta)>=1.0)
return a;
Real halfTheta=acos(cosHalfTheta);
Real sinHalfTheta=sqrt(1.0-cosHalfTheta*cosHalfTheta);
if(abs(sinHalfTheta)<0.001)
return 0.5*a+0.5*b;
Real ratioA=sin((1-t)*halfTheta)/sinHalfTheta;
Real ratioB=sin(t*halfTheta)/sinHalfTheta;
return normalize(ratioA*a+ratioB*b);
}
}
namespace heinlib
{
template<class Real>
void fix_rotation_matrix(static_matrix<3,3,Real>&R,unsigned power_iters=5)
{
static_matrix<4,1,Real>q;
robust_quat_from_matrix(q,R,power_iters);
quat_to_matrix(R,q);
}
template<typename Real>
void standardize_axis_angle(static_matrix<3,1,Real>&axis,Real&angle)
{
if(axis[0]<0)
{
axis*=-1;
angle=constants<Real>::two_pi()-angle;
}
while(angle>constants<Real>::pi())
angle-=constants<Real>::two_pi();
while(angle<0)
angle+=constants<Real>::two_pi();
}
template<typename Real>
void split_axis_angle(static_matrix<3,1,Real>&axis,Real&angle,const static_matrix<3,1,Real>&axis_angle)
{
angle=length(axis_angle);
axis=axis_angle;
standardize_axis_angle(axis,angle);
}
template<class Real>
__forceinline void rotation_matrix(static_matrix<2,2,Real>&R,Real angle_rad)
{
Real cos_theta=cos(angle_rad),sin_theta=sin(angle_rad);
static_matrix<2,2,Real>myR={cos_theta,-sin_theta,sin_theta,cos_theta};
R=myR;
}
template<class Real>
__forceinline void rotation_matrix(
static_matrix<3,3,Real>&R,
Real heading=0,
Real attitude=0,
Real bank=0)
{
static_matrix<3,3,Real>R1,R2,R3;
rotation_matrix(R1,make_vector3<Real>(0,0,1)*heading);
rotation_matrix(R2,make_vector3<Real>(1,0,0)*attitude);
rotation_matrix(R3,make_vector3<Real>(0,1,0)*bank);
R=R3*R2*R1;
}
template<typename Real>
__forceinline static_matrix<3,1,Real>rotate(const static_matrix<3,1,Real>&vec,const static_matrix<3,1,Real>&axis,Real angle_rad)
{
static_matrix<3,3,Real>R;
rotation_matrix(R,normalize(axis)*angle_rad);
return R*vec;
}
template<class Real>
void translation_matrix(static_matrix<4,4,Real>&M,const static_matrix<3,1,Real>&t)
{
fill_identity(M);
M(0,3)=t.x();
M(1,3)=t.y();
M(2,3)=t.z();
}
template<unsigned D,class Real>
static_matrix<D+1,D+1,Real>translation_matrix(const static_matrix<D,1,Real>&t)
{
static_matrix<D+1,D+1,Real>ret;
fill_identity(ret);
for(unsigned i=0;i<D;++i)
ret(i,D)=t[i];
return ret;
}
template<class Real>
__forceinline void rotation_matrix(static_matrix<3,3,Real>&R,const static_matrix<3,1,Real>&axis_angle)
{
Real angle=length(axis_angle);
static_matrix<3,1,Real>v=(angle==0)?axis_angle:axis_angle/angle;
Real c=cos(angle),s=sin(angle),C=1-c;
const Real&x=v.x(),&y=v.y(),&z=v.z();
Real xs=x*s,ys=y*s,zs=z*s,
xC=x*C,yC=y*C,zC=z*C,
xyC=x*yC,yzC=y*zC,zxC=z*xC;
R(0,0)=x*xC+c;R(0,1)=xyC-zs;R(0,2)=zxC+ys;
R(1,0)=xyC+zs;R(1,1)=y*yC+c;R(1,2)=yzC-xs;
R(2,0)=zxC-ys;R(2,1)=yzC+xs;R(2,2)=z*zC+c;
}
template<typename Real>
void matrix_from_axis_angle(static_matrix<3,3,Real>&R,const static_matrix<3,1,Real>&axis,Real theta)
{
Real t1=cos(theta);
Real t2=1-t1;
Real t3=axis.x()*axis.x();
Real t6=t2*axis.x();
Real t7=t6*axis.y();
Real t8=sin(theta);
Real t9=t8*axis.z();
Real t11=t6*axis.z();
Real t12=t8*axis.y();
Real t15=axis.y()*axis.y();
Real t19=t2*axis.y()*axis.z();
Real t20=t8*axis.x();
Real t24=axis.z()*axis.z();
R(0,0)=t1+t2*t3;
R(0,1)=t7-t9;
R(0,2)=t11+t12;
R(1,0)=t7+t9;
R(1,1)=t1+t2*t15;
R(1,2)=t19-t20;
R(2,0)=t11-t12;
R(2,1)=t19+t20;
R(2,2)=t1+t2*t24;
}
template<class Real>
__forceinline void rodrigues_rotation(static_matrix<3,3,Real>&R,const static_matrix<3,1,Real>&vin)
{
Real angle=length(vin);
static_matrix<3,1,Real>v=(angle==0)?vin:vin/angle;
static_matrix<3,3,Real>sk={0,-v.z(),v.y(),
v.z(),0,-v.x(),
-v.y(),v.x(),0};
R=constants<static_matrix<3,3,Real> >::identity();
R+=sk*sin(angle)+(1-cos(angle))*(sk*sk);
}
template<class Real>
void mirror_matrix(static_matrix<4,4,Real>&m,
const static_matrix<3,1,Real>&p,
const static_matrix<3,1,Real>&n)
{
Real k=dot(p,n);
Real nx=n.x(),ny=n.y(),nz=n.z();
static_matrix<4,4,Real>ret={1-2*nx*nx,-2*nx*ny,-2*nx*nz,2*nx*k,
-2*ny*nx,1-2*ny*ny,-2*ny*nz,2*ny*k,
-2*nz*nx,-2*nz*ny,1-2*nz*nz,2*nz*k,
0,0,0,1};
m=ret;
}
template<class Real>
void rotation_between_vectors(
static_matrix<3,1,Real>&axis,
Real&angle,
const static_matrix<3,1,Real>&v1,
const static_matrix<3,1,Real>&v2)
{
static_matrix<3,1,Real>v1n=normalize(v1),v2n=normalize(v2);
static_matrix<3,1,Real>cr=cross(v1n,v2n);
Real len=length(cr);
if(len>0)
{
axis=cr/len;
angle=acos(dot(v1n,v2n));
}else
{
axis.x()=1;
axis.y()=0;
axis.z()=0;
angle=0;
}
}
template<class Real>
__forceinline Real angle(const static_matrix<3,3,Real>&A,const static_matrix<3,3,Real>&B)
{
return acos((trace(transpose(A)*B)-1)*0.5f);
}
template<class Real>
Real geodesic_distance(const static_matrix<3,3,Real>&A,const static_matrix<3,3,Real>&B)
{
Real theta=angle(A,B);
return constants<Real>::sqrt_2()*abs(theta);
}
}
#pragma once
namespace heinlib
{
template<class Matrix,class Distrib>
typename enable_if<is_matrix<Matrix>,void>::type
fill_random(Matrix&A,const Distrib&noise,unsigned row_start=0,unsigned col_start=0,unsigned row_end=0,unsigned col_end=0)
{
if(row_end<=row_start)row_end=A.rows;
if(col_end<=col_start)col_end=A.cols;
for(unsigned i=row_start;i<row_end;++i)
for(unsigned j=col_start;j<col_end;++j)
A(i,j)=noise.sample();
}
template<class Real,class Distrib>
dynamic_matrix<Real>random_matrix(unsigned rows,unsigned cols,const Distrib&noise)
{
dynamic_matrix<Real>A(rows,cols);
fill_random(A,noise);
return A;
}
template<typename MxN>
void fill_random(MxN&A,typename MxN::real_type minv=-1,typename MxN::real_type maxv=1)
{
unsigned elts=num_elements(A);
for(unsigned i=0;i<elts;++i)
A[i]=random<typename MxN::real_type>::uniform(minv,maxv);
}
template<unsigned M,typename Real>
static_matrix<M,1,Real>random_vector(Real minv=-1,Real maxv=1)
{
static_matrix<M,1,Real>x;
fill_random(x,minv,maxv);
return x;
}
template<unsigned M,typename Real,class Distrib>
static_matrix<M,1,Real>random_vector(const Distrib&noise)
{
static_matrix<M,1,Real>x;
fill_random(x,noise);
return x;
}
template<typename Nx1>
inline typename square_result<Nx1>::result_type matrix_from_eigenvalues(const Nx1&eigs)
{
unsigned N=eigs.rows;
typedef typename square_result<Nx1>::result_type NxN;
NxN x,xinv;
resize_if_dynamic(x,N,N);
resize_if_dynamic(xinv,N,N);
while(true)
{
fill_random(x);
if(matrix_inverse(x,xinv))
break;
}
return x*diagonal(eigs)*xinv;
}
template<class Real>
bool matrix_from_left_right_null_spaces(static_matrix<3,3,Real>&M,
const static_matrix<1,3,Real>&L,
const static_matrix<3,1,Real>&R)
{
static_matrix<6,9,Real>C=
{
R[0],R[1],R[2],0,0,0,0,0,0,
0,0,0,R[0],R[1],R[2],0,0,0,
0,0,0,0,0,0,R[0],R[1],R[2],
L[0],0,0,L[1],0,0,L[2],0,0,
0,L[0],0,0,L[1],0,0,L[2],0,
0,0,L[0],0,0,L[1],0,0,L[2]
};
static_matrix<9,1,Real>m;
if(!null_space_SVD(C,m))return false;
matrix_copy(M,m);
return true;
}
template<class Real>
bool matrix_from_right_null_space(static_matrix<3,3,Real>&M,
const static_matrix<3,1,Real>&R)
{
static_matrix<3,9,Real>C=
{
R[0],R[1],R[2],0,0,0,0,0,0,
0,0,0,R[0],R[1],R[2],0,0,0,
0,0,0,0,0,0,R[0],R[1],R[2]
};
static_matrix<9,1,Real>m;
if(!null_space_SVD(C,m))return false;
matrix_copy(M,m);
return true;
}
template<class Real>
bool matrix_from_left_null_space(static_matrix<3,3,Real>&M,
const static_matrix<1,3,Real>&L)
{
static_matrix<3,9,Real>C=
{
L[0],0,0,L[1],0,0,L[2],0,0,
0,L[0],0,0,L[1],0,0,L[2],0,
0,0,L[0],0,0,L[1],0,0,L[2]
};
static_matrix<9,1,Real>m;
if(!null_space_SVD(C,m))return false;
matrix_copy(M,m);
return true;
}
template<unsigned N,unsigned B,class Real>
bool matrix_from_null_space(static_matrix<N,N,Real>&M,const static_matrix<N,B,Real>&Null)
{
for(unsigned i=0;i<B;++i)
if(length_sq(Null.get_column(i))==0)
{
fill(M,0);
return false;
}
for(unsigned i=0;i<B;++i)
for(unsigned j=0;j<N;++j)
M(i,j)=0;
static_matrix<N,1,Real>vec;
for(unsigned row=B;row<N;++row)
{
Real norm=0;
do
{
fill_random(vec);
for(unsigned i=0;i<B;++i){
vec-=project(vec,Null.get_column(i));
}
for(unsigned i=B;i<row;++i){
vec-=project(vec,transpose(M.get_row(i)));
}
norm=length(vec);
}while(norm==0);
vec/=norm;
M.block_insert(row,0,transpose(vec));
}
}
}
#pragma once
namespace heinlib
{
template<class Real,unsigned D>
static_matrix<D,D,Real>rotate_covariance(
const static_matrix<D,D,Real>&covar,
const static_matrix<D,D,Real>&R)
{
return inverse(R)*covar*R;
}
}
