void Utility::SyncMatrix(Movie *movie)
{
int matrixId = movie->matrixId;
float scaleX = 1;
float scaleY = 1;
float rotation = 0;
Matrix matrix;
if ((matrixId & Constant::MATRIX_FLAG) == 0) {
const Translate &translate = movie->lwf->data->translates[matrixId];
matrix.Set(scaleX, scaleY,
0, 0, translate.translateX, translate.translateY);
} else {
matrixId &= ~Constant::MATRIX_FLAG_MASK;
matrix.Set(&movie->lwf->data->matrices[matrixId]);
bool md = GetMatrixDeterminant(&matrix);
scaleX = sqrtf(
matrix.scaleX * matrix.scaleX + matrix.skew1 * matrix.skew1);
if (md)
scaleX = -scaleX;
scaleY = sqrtf(
matrix.scaleY * matrix.scaleY + matrix.skew0 * matrix.skew0);
if (md)
rotation = atan2f(matrix.skew1, -matrix.scaleX);
else
rotation = atan2f(matrix.skew1, matrix.scaleX);
rotation = rotation / M_PI * 180.0f;
}
movie->SetMatrix(&matrix, scaleX, scaleY, rotation);
}
inline void
HermitianSign( UpperOrLower uplo, Matrix<F>& A )
{
#ifndef RELEASE
CallStackEntry entry("HermitianSign");
#endif
typedef BASE(F) R;
// Get the EVD of A
Matrix<R> w;
Matrix<F> Z;
HermitianEig( uplo, A, w, Z );
// Compute the two-norm of A as the maximum absolute value of its eigvals
const R twoNorm = MaxNorm( w );
// Set the tolerance equal to n ||A||_2 eps, and invert values above it
const int n = A.Height();
const R eps = lapack::MachineEpsilon<R>();
const R tolerance = n*twoNorm*eps;
for( int i=0; i<n; ++i )
{
const R omega = w.Get(i,0);
if( Abs(omega) < tolerance )
w.Set(i,0,0);
else if( omega > 0 )
w.Set(i,0,R(1));
else
w.Set(i,0,R(-1));
}
// Reform the Hermitian matrix with the modified eigenvalues
hermitian_function::ReformHermitianMatrix( uplo, A, w, Z );
}
void SOCSquareRoot
( const Matrix<Real>& x,
Matrix<Real>& xRoot,
const Matrix<Int>& orders,
const Matrix<Int>& firstInds )
{
DEBUG_ONLY(CSE cse("SOCSquareRoot"))
Matrix<Real> d;
SOCDets( x, d, orders, firstInds );
ConeBroadcast( d, orders, firstInds );
const Int height = x.Height();
Zeros( xRoot, height, 1 );
for( Int i=0; i<height; )
{
const Int order = orders.Get(i,0);
const Int firstInd = firstInds.Get(i,0);
if( i != firstInd )
LogicError("Inconsistency in orders and firstInds");
const Real eta0 = Sqrt(x.Get(i,0)+Sqrt(d.Get(i,0)))/Sqrt(Real(2));
xRoot.Set( i, 0, eta0 );
for( Int k=1; k<order; ++k )
xRoot.Set( i+k, 0, x.Get(i+k,0)/(2*eta0) );
i += order;
}
}
inline void
MakeKahan( F phi, Matrix<F>& A )
{
#ifndef RELEASE
PushCallStack("MakeKahan");
#endif
typedef typename Base<F>::type R;
const int m = A.Height();
const int n = A.Width();
if( m != n )
throw std::logic_error("Cannot make a non-square matrix Kahan");
if( Abs(phi) >= R(1) )
throw std::logic_error("Phi must be in (0,1)");
const F zeta = Sqrt(1-phi*Conj(phi));
MakeZeros( A );
for( int i=0; i<n; ++i )
{
const F zetaPow = Pow( zeta, R(i) );
A.Set( i, i, zetaPow );
for( int j=1; j<n; ++j )
A.Set( i, j, -phi*zetaPow );
}
#ifndef RELEASE
PopCallStack();
#endif
}
void gen_acquisition
(double srcx, double srcy, double srcz,
Matrix<double>& xRecv,
Matrix<double>& yRecv,
Matrix<double>& zRecv )
{
double srcx=0.5;
double srcy=0.5;
double srcz=0.03;
int nxr=1;
int nyr=1;
int nzr=1;
int nrec=nxr*nyr*nzr;
const double oxr=0.1, oyr=0.1, ozr=0.03;
const double dxr=0.02, dyr=0.02, dzr=0.0;
xRecv.Resize( nrec, nsrc );
yRecv.Resize( nrec, nsrc );
zRecv.Resize( nrec, nsrc );
for (int isrc=0; isrc<nsrc; isrc++) {
for ( int k=0; k<nzr; k++ ) {
for (int j=0; j<nyr; j++) {
for (int i=0; i<nxr; i++) {
xRecv.Set( irec, isrc, oxr+i*dxr );
yRecv.Set( irec, isrc, oyr+j*dyr );
zRecv.Set( irec, isrc, ozr+k*dzr );
}
}
}
}
}
const Matrix unitMatrix()
{
Matrix one;
one.Set(0,0, 1.0);
one.Set(1,1, 1.0);
one.Set(2,2, 1.0);
return one;
}
inline Complex<R>
Reflector( Matrix<Complex<R> >& chi, Matrix<Complex<R> >& x )
{
#ifndef RELEASE
CallStackEntry entry("Reflector");
#endif
typedef Complex<R> C;
R norm = Nrm2( x );
C alpha = chi.Get(0,0);
if( norm == 0 && alpha.imag == R(0) )
{
chi.Set(0,0,-chi.Get(0,0));
return C(2);
}
R beta;
if( alpha.real <= 0 )
beta = lapack::SafeNorm( alpha.real, alpha.imag, norm );
else
beta = -lapack::SafeNorm( alpha.real, alpha.imag, norm );
const R one = 1;
const R safeMin = lapack::MachineSafeMin<R>();
const R epsilon = lapack::MachineEpsilon<R>();
const R safeInv = safeMin/epsilon;
Int count = 0;
if( Abs(beta) < safeInv )
{
R invOfSafeInv = one/safeInv;
do
{
++count;
Scale( invOfSafeInv, x );
alpha *= invOfSafeInv;
beta *= invOfSafeInv;
} while( Abs(beta) < safeInv );
norm = Nrm2( x );
if( alpha.real <= 0 )
beta = lapack::SafeNorm( alpha.real, alpha.imag, norm );
else
beta = -lapack::SafeNorm( alpha.real, alpha.imag, norm );
}
C tau = C( (beta-alpha.real)/beta, -alpha.imag/beta );
Scale( one/(alpha-beta), x );
for( Int j=0; j<count; ++j )
beta *= safeInv;
chi.Set(0,0,beta);
return tau;
}
// outer product in place
void outer_product( const Vector& v1, const Vector& v2, Matrix& m )
{
m.Set( 0, 0, v1(0)*v2(0) );
m.Set( 0, 1, v1(0)*v2(1) );
m.Set( 0, 2, v1(0)*v2(2) );
m.Set( 1, 0, v1(1)*v2(0) );
m.Set( 1, 1, v1(1)*v2(1) );
m.Set( 1, 2, v1(1)*v2(2) );
m.Set( 2, 0, v1(2)*v2(0) );
m.Set( 2, 1, v1(2)*v2(1) );
m.Set( 2, 2, v1(2)*v2(2) );
}
inline void
Lehmer( Matrix<F>& L, Int n )
{
DEBUG_ONLY(CallStackEntry cse("Lehmer"))
L.Resize( n, n );
for( Int j=0; j<n; ++j )
{
for( Int i=0; i<j; ++i )
L.Set( i, j, F(i+1)/F(j+1) );
for( Int i=j; i<n; ++i )
L.Set( i, j, F(j+1)/F(i+1) );
}
}
inline R
Reflector( Matrix<R>& chi, Matrix<R>& x )
{
#ifndef RELEASE
CallStackEntry entry("Reflector");
#endif
R norm = Nrm2( x );
if( norm == 0 )
{
chi.Set(0,0,-chi.Get(0,0));
return R(2);
}
R beta;
R alpha = chi.Get(0,0);
if( alpha <= 0 )
beta = lapack::SafeNorm( alpha, norm );
else
beta = -lapack::SafeNorm( alpha, norm );
const R one = 1;
const R safeMin = lapack::MachineSafeMin<R>();
const R epsilon = lapack::MachineEpsilon<R>();
const R safeInv = safeMin/epsilon;
Int count = 0;
if( Abs(beta) < safeInv )
{
R invOfSafeInv = one/safeInv;
do
{
++count;
Scale( invOfSafeInv, x );
alpha *= invOfSafeInv;
beta *= invOfSafeInv;
} while( Abs(beta) < safeInv );
norm = Nrm2( x );
if( alpha <= 0 )
beta = lapack::SafeNorm( alpha, norm );
else
beta = -lapack::SafeNorm( alpha, norm );
}
R tau = (beta-alpha) / beta;
Scale( one/(alpha-beta), x );
for( Int j=0; j<count; ++j )
beta *= safeInv;
chi.Set(0,0,beta);
return tau;
}
inline void
MakeJordan( Matrix<T>& J, T lambda )
{
DEBUG_ONLY(CallStackEntry cse("MakeJordan"))
Zero( J );
const Int m = J.Height();
const Int n = J.Width();
for( Int j=0; j<std::min(m,n); ++j )
{
J.Set( j, j, lambda );
if( j != 0 )
J.Set( j-1, j, T(1) );
}
}
inline void
Riemann( Matrix<T>& R, Int n )
{
DEBUG_ONLY(CallStackEntry cse("Riemann"))
R.Resize( n, n );
for( Int j=0; j<n; ++j )
{
for( Int i=0; i<n; ++i )
{
if( ((j+2)%(i+2))==0 )
R.Set( i, j, T(i+1) );
else
R.Set( i, j, T(-1) );
}
}
}
Matrix& Matrix::operator*(const Matrix& mat)
{
Matrix *temp = new Matrix;
temp->MM = this->MM;
temp->NN = this->NN;
temp->data = new double[temp->MM * temp->NN];
int i = 0, j = 0, m = 0, n = 0, x = 0, y = 0;
double a = 0, b = 0, c = 0;
while (x < temp->MM)
{
for (i = 0; i < this->MM; i++)
{
for (j = 0; j < this->NN; j++)
{
if (m > mat.MM)
{
m = 0;
n++;
temp->Set(x, y, c);
y++;
}
a = this->Get(i, j);
b = mat.Get(m, n);
c += a*b;
m++;
}
}
}
return *temp;
}
//--------------------------------------------------------------------------------------
Value* Matrix::Clone() const
{
HRESULT hr;
Matrix* pNew = new Matrix( Type(), Rows(), Columns() );
if( NULL == pNew )
{
// TODO - Error string
return NULL;
}
for( UINT r=0; r < Rows(); r++ )
{
for( UINT c=0; c < Columns(); c++ )
{
hr = pNew->Set( r, c, Get(r, c) );
if( FAILED(hr) )
{
// TODO - Error string
SAFE_DELETE(pNew);
return NULL;
}
}
}
return pNew;
}
double gen_adjsrc
( const Matrix<Complex<double>>& S,
const Matrix<Complex<double>>& O,
Matrix<Complex<double>>& A )
{
// TODO: Compute local misfits and then sum the result with an
// MPI_Allreduce summation
// NOTE: This routine should already work but may not be as fast
// as possible.
double l2MisfitSquared=0.0;
const int nsrc = S.Height();
const int nrec = S.Width();
A.Resize( nsrc, nrec );
for (int isrc=0;isrc<nsrc;isrc++ ) {
for (int irec=0;irec<nrec;irec++ ) {
Complex<double> obs = O.Get(irec,isrc);
Complex<double> syn = S.Get(irec,isrc);
A.Set( irec, isrc, obs-syn );
l2MisfitSquared += Abs(A.Get(irec,isrc));
}
}
return l2MisfitSquared;
}
inline void
SortEig( Matrix<R>& w, Matrix<R>& Z )
{
#ifndef RELEASE
PushCallStack("SortEig");
#endif
const int n = Z.Height();
const int k = Z.Width();
// Initialize the pairs of indices and eigenvalues
std::vector<internal::IndexValuePair<R> > pairs( k );
for( int i=0; i<k; ++i )
{
pairs[i].index = i;
pairs[i].value = w.Get(i,0);
}
// Sort the eigenvalues and simultaneously form the permutation
std::sort
( pairs.begin(), pairs.end(), internal::IndexValuePair<R>::Compare );
// Reorder the eigenvectors and eigenvalues using the new ordering
Matrix<R> ZPerm( n, k );
for( int j=0; j<k; ++j )
{
const int source = pairs[j].index;
MemCopy( ZPerm.Buffer(0,j), Z.LockedBuffer(0,source), n );
w.Set(j,0,pairs[j].value);
}
Z = ZPerm;
#ifndef RELEASE
PopCallStack();
#endif
}
void Camera::GetMatrix(Matrix &m)
{
if (dirty)
GetMatrix();
m.Set(matrix);
}
inline void
Cauchy
( const std::vector<F>& x, const std::vector<F>& y, Matrix<F>& A )
{
#ifndef RELEASE
PushCallStack("Cauchy");
#endif
const int m = x.size();
const int n = y.size();
A.ResizeTo( m, n );
const F one = F(1);
for( int j=0; j<n; ++j )
{
for( int i=0; i<m; ++i )
{
#ifndef RELEASE
// TODO: Use tolerance instead?
if( x[i] == y[j] )
{
std::ostringstream msg;
msg << "x[" << i << "] = y[" << j << "] (" << x[i]
<< ") is not allowed for Cauchy matrices";
throw std::logic_error( msg.str().c_str() );
}
#endif
A.Set( i, j, one/(x[i]-y[j]) );
}
}
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
Redheffer( Matrix<T>& R, Int n )
{
DEBUG_ONLY(CallStackEntry cse("Redheffer"))
R.Resize( n, n );
for( Int j=0; j<n; ++j )
{
for( Int i=0; i<n; ++i )
{
if( j==0 || ((j+1)%(i+1))==0 )
R.Set( i, j, T(1) );
else
R.Set( i, j, T(0) );
}
}
}
void Forsythe( Matrix<T>& J, Int n, T alpha, T lambda )
{
DEBUG_ONLY(CSE cse("Forsythe"))
Jordan( J, n, lambda );
if( n > 0 )
J.Set( n-1, 0, alpha );
}
void Lotkin( Matrix<F>& A, Int n )
{
DEBUG_ONLY(CallStackEntry cse("Lotkin"))
Hilbert( A, n );
// Set first row to all ones
for( Int j=0; j<n; ++j )
A.Set( 0, j, F(1) );
}
inline void
Walsh( int k, Matrix<T>& A, bool binary )
{
#ifndef RELEASE
PushCallStack("Walsh");
#endif
if( k < 1 )
throw std::logic_error("Walsh matrices are only defined for k>=1");
const unsigned n = 1u<<k;
A.ResizeTo( n, n );
// Run a simple O(n^2 log n) algorithm for computing the entries
// based upon successive sign flips
const T onValue = 1;
const T offValue = ( binary ? 0 : -1 );
for( unsigned j=0; j<n; ++j )
{
for( unsigned i=0; i<n; ++i )
{
// Recurse on the quadtree, flipping the sign of the entry each
// time we are in the bottom-right quadrant
unsigned r = i;
unsigned s = j;
unsigned t = n;
bool on = true;
while( t != 1u )
{
t >>= 1;
if( r >= t && s >= t )
on = !on;
r %= t;
s %= t;
}
if( on )
A.Set( i, j, onValue );
else
A.Set( i, j, offValue );
}
}
#ifndef RELEASE
PopCallStack();
#endif
}
inline void
Redheffer( Matrix<T>& R, Int n )
{
#ifndef RELEASE
CallStackEntry entry("Redheffer");
#endif
R.ResizeTo( n, n );
for( Int j=0; j<n; ++j )
{
for( Int i=0; i<n; ++i )
{
if( j==0 || ((j+1)%(i+1))==0 )
R.Set( i, j, T(1) );
else
R.Set( i, j, T(0) );
}
}
}
inline void
Riemann( Matrix<T>& R, int n )
{
#ifndef RELEASE
CallStackEntry entry("Riemann");
#endif
R.ResizeTo( n, n );
for( int j=0; j<n; ++j )
{
for( int i=0; i<n; ++i )
{
if( ((j+2)%(i+2))==0 )
R.Set( i, j, T(i+1) );
else
R.Set( i, j, T(-1) );
}
}
}
///////////////////////////////////////////////////////////////////////////////
//
// Trig
//
static void Fill(Matrix &m)
{
Quaternion q;
q.Set(Random::nonSync.Float() * PI * 2.0F, Matrix::I.up);
q.Rotate(Random::nonSync.Float() * PI * 2.0F, Matrix::I.right);
q.Rotate(Random::nonSync.Float() * PI * 2.0F, Matrix::I.front);
m.Set(q);
}
inline void
Pseudoinverse( Matrix<F>& A )
{
#ifndef RELEASE
PushCallStack("Pseudoinverse");
#endif
typedef typename Base<F>::type R;
const int m = A.Height();
const int n = A.Width();
const int k = std::max(m,n);
// Get the SVD of A
Matrix<R> s;
Matrix<F> U, V;
U = A;
SVD( U, s, V );
// Compute the two-norm of A as the maximum singular value
const R twoNorm = Norm( s, INFINITY_NORM );
// Set the tolerance equal to k ||A||_2 eps and invert above tolerance
const R eps = lapack::MachineEpsilon<R>();
const R tolerance = k*twoNorm*eps;
const int numVals = s.Height();
for( int i=0; i<numVals; ++i )
{
const R sigma = s.Get(i,0);
if( sigma < tolerance )
s.Set(i,0,0);
else
s.Set(i,0,1/sigma);
}
// Scale U with the singular values, U := U Sigma
DiagonalScale( RIGHT, NORMAL, s, U );
// Form pinvA = (U Sigma V^H)^H = V (U Sigma)^H
Zeros( n, m, A );
Gemm( NORMAL, ADJOINT, F(1), V, U, F(0), A );
#ifndef RELEASE
PopCallStack();
#endif
}
void L( Matrix<F>& A, Matrix<F>& t )
{
#ifndef RELEASE
CallStackEntry entry("hermitian_tridiag::L");
if( A.Height() != A.Width() )
LogicError("A must be square");
#endif
typedef BASE(F) R;
const Int tHeight = Max(A.Height()-1,0);
t.ResizeTo( tHeight, 1 );
// Matrix views
Matrix<F>
ATL, ATR, A00, a01, A02, alpha21T,
ABL, ABR, a10, alpha11, a12, a21B,
A20, a21, A22;
// Temporary matrices
Matrix<F> w21;
PartitionDownDiagonal
( A, ATL, ATR,
ABL, ABR, 0 );
while( ATL.Height()+1 < A.Height() )
{
RepartitionDownDiagonal
( ATL, /**/ ATR, A00, /**/ a01, A02,
/*************/ /**********************/
/**/ a10, /**/ alpha11, a12,
ABL, /**/ ABR, A20, /**/ a21, A22, 1 );
PartitionDown
( a21, alpha21T,
a21B, 1 );
//--------------------------------------------------------------------//
const F tau = Reflector( alpha21T, a21B );
const R epsilon1 = alpha21T.GetRealPart(0,0);
t.Set(A00.Height(),0,tau);
alpha21T.Set(0,0,F(1));
Zeros( w21, a21.Height(), 1 );
Hemv( LOWER, tau, A22, a21, F(0), w21 );
const F alpha = -tau*Dot( w21, a21 )/F(2);
Axpy( alpha, a21, w21 );
Her2( LOWER, F(-1), a21, w21, A22 );
alpha21T.Set(0,0,epsilon1);
//--------------------------------------------------------------------//
SlidePartitionDownDiagonal
( ATL, /**/ ATR, A00, a01, /**/ A02,
/**/ a10, alpha11, /**/ a12,
/*************/ /**********************/
ABL, /**/ ABR, A20, a21, /**/ A22 );
}
}
inline void
MakeIdentity( Matrix<T>& I )
{
DEBUG_ONLY(CallStackEntry cse("MakeIdentity"))
Zero( I );
const Int m = I.Height();
const Int n = I.Width();
for( Int j=0; j<std::min(m,n); ++j )
I.Set( j, j, T(1) );
}
PerspectiveCamera::PerspectiveCamera(Vec3f ¢er, Vec3f &direction, Vec3f &up, float angle){
Matrix translationMatrix;
Vec3f u,v,w;
_center = center;
_z = 1/(tan(angle/2));
w = direction;
w.Normalize();
Vec3f::Cross3(u,up,direction);
u.Normalize();
Vec3f::Cross3(v,w,u);
_viewMatrix = new Matrix();
_viewMatrix->Set(0,0,u.x());
_viewMatrix->Set(1,0,u.y());
_viewMatrix->Set(2,0,u.z());
_viewMatrix->Set(0,1,v.x());
_viewMatrix->Set(1,1,v.y());
_viewMatrix->Set(2,1,v.z());
_viewMatrix->Set(0,2,w.x());
_viewMatrix->Set(1,2,w.y());
_viewMatrix->Set(2,2,w.z());
_viewMatrix->Set(3,3,1);
translationMatrix.Set(0,0,1);
translationMatrix.Set(1,1,1);
translationMatrix.Set(2,2,1);
translationMatrix.Set(3,3,1);
translationMatrix.Set(3,0,-center.x());
translationMatrix.Set(3,1,-center.y());
translationMatrix.Set(3,2,-center.z());
(*_viewMatrix) = (*_viewMatrix)*translationMatrix;
_viewMatrix->Inverse();
}
const Matrix Matrix::exp() const
{
Matrix expm;
expm.Set(0,0,1);
expm.Set(1,1,1);
expm.Set(2,2,1);
Matrix mc(*this);
double factorial = 1;
const int terms = 25;
for(int t=1; t<=terms; t++)
{
factorial *= t;
expm += mc / factorial;
mc *= (*this);
}
return expm;
}
