void cast_and_apply(Function& f, Matrix1& first, Matrix2& second)
{
typedef typename Matrix2::value_type value_type;
typedef typename Matrix2::dense_type dense_type;
typedef typename Matrix2::sparse_type sparse_type;
typedef typename Matrix2::scalar_type scalar_type;
if (second.is_true_scalar()) { // kanske bara ta in värdet ist
const scalar_type& sc = dynamic_cast<const scalar_type&> (second);
Operator<Function, Matrix1, const scalar_type> op;
op(f, first, sc);
}
else if (second.is_dense()) {
const dense_type& d = dynamic_cast<const dense_type&> (second);
Operator<Function, Matrix1, const dense_type> op;
op(f, first, d);
}
else if (second.is_sparse()) {
const sparse_type& s = dynamic_cast<const sparse_type&> (second);
Operator<Function, Matrix1, const sparse_type> op;
op(f, first, s);
}
else {
Operator<Function, Matrix1, const Matrix2> op;
op(f, first, second);
}
}
Matrix ifft2_1D(const Matrix2& mat)
{
fftw_complex* data_in;
fftw_complex* ifft;
fftw_plan plan_b;
data_in = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * mat.imge.size1());
ifft = (fftw_complex*)fftw_malloc(sizeof(fftw_complex) * mat.imge.size1());
plan_b = fftw_plan_dft_1d(mat.imge.size1(), data_in, ifft,
FFTW_BACKWARD, FFTW_ESTIMATE);
for(int i = 0, k = 0; i < mat.imge.size1(); ++i)
{
data_in[k][0] = mat.real(i, 0);
data_in[k][1] = mat.imge(i, 0);
k++;
}
/* perform FFT */
fftw_execute(plan_b);
double normal_val = 1.0 / (mat.imge.size1() * mat.imge.size2());
Matrix out(mat.imge.size1(), mat.imge.size2());
for(int i = 0, k = 0; i < mat.imge.size1(); ++i)
{
for(int j = 0; j < mat.imge.size2(); ++j)
{
out(i, j) = ifft[k][0] * normal_val;
k++;
}
}
fftw_destroy_plan(plan_b);
fftw_free(data_in);
fftw_free(ifft);
return out;
}
ResultType matrixByMatrix(const Matrix1& a1, const Matrix2& a2)
{
ResultType result(a1.numberOfRows(),a2.numberOfColumns(),true);
if(a1.numberOfColumns()!=a2.numberOfRows())
throw std::range_error("operator chomp::vectalg::matrixByMatrix: incompatible matrix dimensions");
for(int i=0;i<result.numberOfColumns();++i)
{
typename ResultType::iterator b=result.begin()+i, e=result.end()+i;
typename Matrix1::const_iterator j=a1.begin();
while(b!=e)
{
typename Matrix2::const_iterator b1=a2.begin()+i, e1=a2.end()+i;
typename ResultType::ScalarType x = TypeTraits<typename ResultType::ScalarType>::zero();
while(b1!=e1)
{
x += (*j) * (*b1);
++j;
b1+=a2.rowStride();
}
*b=x;
b+=result.rowStride();
}
}
return result;
}
Vector2 RandomiseDirectionVector(const Vector2 &direction, float halfAngle)
{
float angle = RandFloatSigned() * halfAngle;
Matrix2 rotation;
rotation.FromRotation(angle);
return rotation * direction;
}
std::size_t cyclic_eigen_jacobi( const Matrix1& A, Matrix2& V, Otor o, std::size_t max_rot = 80, const T eps = T( 1.0e-10 ) )
{
typedef typename Matrix1::value_type value_type;
typedef typename Matrix1::size_type size_type;
auto const compare_func = [eps]( const value_type lhs, const value_type rhs ) { return std::abs(lhs-rhs) < eps; };
assert( A.row() == A.col() );
//assert( is_symmetric( A, compare_func ) );
size_type i = 0;
auto a = A;
auto const n = a.row();
auto const one = value_type( 1 );
auto const zero = value_type( 0 );
// @V = diag{1, 1, ..., 1}
V.resize( n, n );
V = zero;
std::fill( V.diag_begin(), V.diag_end(), one );
for ( ; i != max_rot; ++i )
{
if ( !(i&7) && eigen_jacobi_private::norm(a) == zero )
{
break;
}
for ( size_type p = 0; p != n; ++p )
for ( size_type q = p+1; q != n; ++q )
eigen_jacobi_private::rotate(a, V, p, q);
}
std::copy( a.diag_begin(), a.diag_end(), o );
return i*n*n;
}
void cast_and_apply(Matrix1& first, Matrix2& second)
{
typedef typename Matrix2::dense_type dense_type;
typedef typename Matrix2::sparse_type sparse_type;
typedef typename Matrix2::scalar_type scalar_type;
if (second.is_true_scalar()) { // kanske bara ta in värdet ist
const scalar_type& sc = dynamic_cast<const scalar_type&> (second);
Operator<Matrix1, const scalar_type> op;
op(first, sc);
}
else if (second.is_dense()) {
const dense_type& d = dynamic_cast<const dense_type&> (second);
Operator<Matrix1, const dense_type> op;
op(first, d);
}
else if (second.is_sparse()) {
const sparse_type& s = dynamic_cast<const sparse_type&> (second);
Operator<Matrix1, const sparse_type> op;
op(first, s);
}
else {
std::cout << "Did not match a type" << std::endl;
Operator<Matrix1, const Matrix2> op;
op(first, second);
}
}
inline Vector2<T_Scalar> operator*(const Vector2<T_Scalar>& v, const Matrix2<T_Scalar>& m)
{
Vector2<T_Scalar> t;
t.x() = v.x()*m.e(0,0) + v.y()*m.e(1,0);
t.y() = v.x()*m.e(0,1) + v.y()*m.e(1,1);
return t;
}
bool testaxpycblasConsistency (LELA::Context<Ring, Modules1> &ctx1,
LELA::Context<Ring, Modules2> &ctx2,
const char *text,
const typename Ring::Element &a,
const Matrix1 &A1, const Matrix2 &A2,
const Matrix3 &A3, const Matrix4 &A4)
{
ostringstream str;
str << "Testing " << text << " axpy consistency" << std::ends;
commentator.start (str.str ().c_str ());
std::ostream &report = commentator.report (Commentator::LEVEL_NORMAL, INTERNAL_DESCRIPTION);
bool pass = true;
typename Matrix3::ContainerType A6 (A1.rowdim (), A1.coldim ());
BLAS3::copy(ctx1, A1, A6);
typename Matrix2::ContainerType A7 (A2.rowdim (), A2.coldim ());
typename Matrix4::ContainerType A8 (A2.rowdim (), A2.coldim ());
BLAS3::copy(ctx1, A2, A7);
BLAS3::copy(ctx1, A2, A8);
report << "Matrix A_1: "<< std::endl;
BLAS3::write (ctx1, report, A1);
report << "Matrix A_2: "<< std::endl;
BLAS3::write (ctx1, report, A7);
BLAS3::axpy (ctx1, a, A1, A7);
report << "Matrix a A_1 + A_2: " << std::endl;;
BLAS3::write (ctx1, report, A7);
report << "Matrix A_1: "<< std::endl;
BLAS3::write (ctx1, report, A6);
report << "Matrix A_2: "<< std::endl;
BLAS3::write (ctx1, report, A8);
BLAS3::axpy (ctx2, a, A6, A8);
report << "Matrix a A_3 + A_4: " << std::endl;
BLAS3::write (ctx2, report, A8);
if (!BLAS3::equal (ctx1, A7, A8))
{
commentator.report (Commentator::LEVEL_IMPORTANT, INTERNAL_ERROR)
<< "ERROR: a A_1 + A_2 != a A_3 + A_4 " << std::endl;
pass = false;
}
commentator.stop (MSG_STATUS (pass));
return pass;
}
//-----------------------------------------------------------------------------
Matrix2 operator-(const Matrix2& m) const
{
Matrix2 t;
for(int i=0; i<2; ++i)
for(int j=0; j<2; ++j)
t.e(j,i) = e(j,i) - m.e(j,i);
return t;
}
//-----------------------------------------------------------------------------
Matrix2 operator-() const
{
Matrix2 t;
for(int i=0; i<2; ++i)
for(int j=0; j<2; ++j)
t.e(j,i) = -e(j,i);
return t;
}
Point2D Point2D::rotateby(Matrix2 rm) {
double new_x = rm.m11()*x() + rm.m12()*y();
double new_y = rm.m21()*x() + rm.m22()*y();
return Point2D(new_x, new_y);
};
//-----------------------------------------------------------------------------
Matrix2 getTransposed() const
{
Matrix2 m;
for(int i=0; i<2; ++i)
for(int j=0; j<2; ++j)
m.e(j,i) = e(i,j);
return m;
}
//-----------------------------------------------------------------------------
Matrix2 operator*(T_Scalar d) const
{
Matrix2 t;
for(int i=0; i<2; ++i)
for(int j=0; j<2; ++j)
t.e(j,i) = e(j,i) * d;
return t;
}
void Eigenvalues(const Matrix2& A,Complex& lambda1,Complex& lambda2)
{
Real trace=A.trace();
Real det=A.determinant();
Complex temp2 = Sqr(trace) - 4.0*det;
Complex temp = Sqrt(temp2);
lambda1 = 0.5*(Complex(trace) + temp);
lambda2 = 0.5*(Complex(trace) - temp);
}
// Calculations
static int transpose(lua_State *L)
{
int n = lua_gettop(L); // Number of arguments
if (n != 1)
return luaL_error(L, "Got %d arguments expected 1 (self)", n);
Matrix2* matrix = checkMatrix2(L);
Matrix2_push(L,1, new Matrix2(matrix->getTranspose()));
return 1;
}
static int transformVector(lua_State *L)
{
int n = lua_gettop(L); // Number of arguments
if (n != 2)
return luaL_error(L, "Got %d arguments expected 2 (self, point)", n);
Matrix2* matrix = checkMatrix2(L);
Vector2 v = Vector2_pull(L,2);
Vector2_push(L, matrix->transformVector(v));
return 1;
}
//-----------------------------------------------------------------------------
T_Scalar diff(const Matrix2& other) const
{
T_Scalar err = 0;
for(int i=0; i<2; ++i)
for(int j=0; j<2; ++j)
if (e(j,i) > other.e(j,i)) // avoid fabs/abs
err += e(j,i) - other.e(j,i);
else
err += other.e(j,i) - e(j,i);
return err;
}
/**
* Example of a test. To be completed.
*
*/
bool testEigenDecomposition()
{
unsigned int nbok = 0;
unsigned int nb = 0;
typedef EigenDecomposition<2,double> Eigen2;
typedef Eigen2::Vector Vector2;
typedef Eigen2::Matrix Matrix2;
trace.beginBlock ( "Testing block ..." );
// [4 1]
// [1 2]
Matrix2 A;
A.setComponent( 0, 0, 4 );
A.setComponent( 0, 1, 1 );
A.setComponent( 1, 0, 1 );
A.setComponent( 1, 1, 2 );
Matrix2 P;
Vector2 v;
Eigen2::getEigenDecomposition( A, P, v );
trace.info() << "Input matrix: " << A << std::endl;
trace.info() << "Eigenvectors: " << P << std::endl;
trace.info() << "Eigenvalues: " << v << std::endl;
Vector2 V0 = P.column( 0 );
Vector2 V1 = P.column( 1 );
Vector2 V0_exp( 0.3826834323650898, -0.9238795325112868 );
Vector2 V1_exp( 0.9238795325112868, 0.3826834323650898);
double v0_exp = 1.585786437626905;
double v1_exp = 4.414213562373095;
double error_V0 = (V0-V0_exp).norm();
double error_V1 = (V1-V1_exp).norm();
double error_v0 = fabs( v[0] - v0_exp );
double error_v1 = fabs( v[1] - v1_exp );
trace.info() << "error_V0 = " << error_V0 << std::endl;
trace.info() << "error_V1 = " << error_V1 << std::endl;
trace.info() << "error_v0 = " << error_v0 << std::endl;
trace.info() << "error_v1 = " << error_v1 << std::endl;
double epsilon = 1e-10;
++nb, nbok += error_V0 < epsilon ? 1 : 0;
trace.info() << "(" << nbok << "/" << nb << ") "
<< "error_V0 < epsilon, i.e. " << error_V0 << " < " << epsilon << std::endl;
++nb, nbok += error_V1 < epsilon ? 1 : 0;
trace.info() << "(" << nbok << "/" << nb << ") "
<< "error_V1 < epsilon, i.e. " << error_V1 << " < " << epsilon << std::endl;
++nb, nbok += error_v0 < epsilon ? 1 : 0;
trace.info() << "(" << nbok << "/" << nb << ") "
<< "error_v0 < epsilon, i.e. " << error_v0 << " < " << epsilon << std::endl;
++nb, nbok += error_v1 < epsilon ? 1 : 0;
trace.info() << "(" << nbok << "/" << nb << ") "
<< "error_v1 < epsilon, i.e. " << error_v1 << " < " << epsilon << std::endl;
trace.endBlock();
return nbok == nb;
}
bool Eigenvalues(const Matrix2& A,Real& lambda1,Real& lambda2)
{
Real trace=A.trace();
Real det=A.determinant();
Real temp2 = Sqr(trace) - 4.0*det;
if(temp2 < 0) return false;
Real temp=Sqrt(temp2);
lambda1 = 0.5*(trace + temp);
lambda2 = 0.5*(trace - temp);
return true;
}
Matrix MatAbsPow2(Matrix2& mat)
{
Matrix out = ZMatrix(mat.real.size1(), mat.real.size2());
for(int i = 0; i < out.size1(); ++i)
{
for(int j = 0; j < out.size2(); ++j)
{
out(i, j) = mat.real(i, j) * mat.real(i, j) + mat.imge(i, j) * mat.imge(i, j);
}
}
return out;
}
void output(
const std::string& filename,
const std::string& rowname,
const std::string& colname,
const std::string& value1name,
const Matrix1& matrix1,
const std::string& value2name,
const Matrix2& matrix2
){
int colmin = min(matrix1.colmin(),matrix2.colmin());
int colmax = max(matrix1.colmax(),matrix2.colmax());
int rowmin = min(matrix1.rowmin(),matrix2.rowmin());
int rowmax = max(matrix1.rowmax(),matrix2.rowmax());
output(filename,rowname,rowmin,rowmax,colname,colmin,colmax,value1name,matrix1,value2name,matrix2);
}
Vector2 ResultScreen::GetVecInRect(const Rect & rect, float32 angleInRad)
{
Vector2 retVec;
Matrix2 m;
m.BuildRotation(angleInRad);
angleInRad += DAVA::PI_05;
while(angleInRad > DAVA::PI_05)
angleInRad -= DAVA::PI_05;
if(angleInRad > DAVA::PI_05 / 2)
angleInRad = DAVA::PI_05 - angleInRad;
Vector2 v = Vector2((Point2f(rect.GetSize().x / 2, 0) * m).data) / Abs(cosf(angleInRad));
retVec = v + rect.GetCenter();
return retVec;
}
//-----------------------------------------------------------------------------
T_Scalar getInverse(Matrix2& dest) const
{
if (&dest == this)
{
Matrix2 tmp;
T_Scalar det = getInverse(tmp);
dest = tmp;
return det;
}
else
{
const T_Scalar& a11 = e(0,0);
const T_Scalar& a12 = e(1,0);
const T_Scalar& a21 = e(0,1);
const T_Scalar& a22 = e(1,1);
dest.fill(0);
T_Scalar det = a11*a22-a12*a21;
if (det != 0)
dest = Matrix2(+a22, -a12, -a21, +a11) / det;
return det;
}
}
void Sample(Vector& res)
{
res.resize(3);
Real u=Rand()*TwoPi;
Real sx=Cos(u)*secondaryRadius+primaryRadius,z=Sin(u)*secondaryRadius;
Real theta=Rand()*TwoPi;
Matrix2 R; R.setRotate(theta);
res[0] = R(0,0)*sx;
res[1] = R(1,0)*sx;
res[2] = z;
//TEMP: test
Vector v;
Eval(res,v);
Assert(FuzzyZero(v[0]));
}
// Accessors
static int at(lua_State *L)
{
int n = lua_gettop(L); // Number of arguments
if (n != 3)
return luaL_error(L, "Got %d arguments expected 3 (self, row, col)", n);
Matrix2* matrix = checkMatrix2(L);
// Rows and columns are numbered from 1 to 3 like arrays in lua, so we need to
// substract 1
int row = luaL_checkinteger(L,2)-1;
int col = luaL_checkinteger(L,3)-1;
lua_pushnumber(L, matrix->at(row, col));
return 1;
}
EllipseFit2<Real>::EllipseFit2 (int numPoints, const Vector2<Real>* points,
Vector2<Real>& center, Matrix2<Real>& rotate, Real diag[2],
Real& error)
:
mNumPoints(numPoints),
mPoints(points)
{
// Energy function is E : R^5 -> R where
// V = (V0, V1, V2, V3, V4)
// = (D[0], D[1], U.X(), U.Y(), atan2(R[1][0],R[1][1])).
mTemp = new1<Vector2<Real> >(numPoints);
MinimizeN<Real> minimizer(5, Energy, 8, 8, 32, this);
InitialGuess(numPoints, mPoints, center, rotate, diag);
Real angle = Math<Real>::ACos(rotate[0][0]);
Real e0 = diag[0]*Math<Real>::FAbs(rotate[0][0]) +
diag[1]*Math<Real>::FAbs(rotate[0][1]);
Real e1 = diag[0]*Math<Real>::FAbs(rotate[1][0]) +
diag[1]*Math<Real>::FAbs(rotate[1][1]);
Real v0[5] =
{
((Real)0.5)*diag[0],
((Real)0.5)*diag[1],
center.X() - e0,
center.Y() - e1,
(Real)0
};
Real v1[5] =
{
((Real)2)*diag[0],
((Real)2)*diag[1],
center.X() + e0,
center.Y() + e1,
Math<Real>::PI
};
Real vInitial[5] =
{
diag[0],
diag[1],
center.X(),
center.Y(),
angle
};
Real vMin[5];
minimizer.GetMinimum(v0, v1, vInitial, vMin, error);
diag[0] = vMin[0];
diag[1] = vMin[1];
center.X() = vMin[2];
center.Y() = vMin[3];
rotate.MakeRotation(vMin[4]);
delete1(mTemp);
}
typename boost::enable_if_c< is_readable_matrix<Matrix1>::value &&
is_readable_matrix<Matrix2>::value,
bool >::type is_equal_mat(const Matrix1& M1, const Matrix2& M2, typename mat_traits<Matrix1>::value_type NumTol = typename mat_traits<Matrix1>::value_type(1E-8) ) {
if( ( M1.get_row_count() != M2.get_row_count() ) ||
( M1.get_col_count() != M2.get_col_count() ) )
return false;
typedef typename mat_traits<Matrix1>::size_type SizeType;
using std::fabs;
for(SizeType i = 0; i < M1.get_row_count(); ++i)
for(SizeType j = 0; j < M1.get_col_count(); ++j)
if( fabs(M1(i,j) - M2(i,j)) > NumTol )
return false;
return true;
};
std::size_t eigen_jacobi( const Matrix1& A, Matrix2& V, Otor o, const T eps = T( 1.0e-10 ) )
{
typedef typename Matrix1::value_type value_type;
typedef typename Matrix1::size_type size_type;
assert( A.row() == A.col() );
//assert( is_symmetric( A ) );
auto a = A;
auto const n = a.row();
auto const one = value_type( 1 );
auto const zero = value_type( 0 );
// @V = diag{1, 1, ..., 1}
V.resize( n, n );
V = zero;
std::fill( V.diag_begin(), V.diag_end(), one );
#if 1
for ( size_type i = 0; i != size_type( -1 ); ++i )
{
// @find max non-diag value in A
size_type p = 0;
size_type q = 1;
value_type current_max = std::abs( a[p][q] );
for ( size_type ip = 0; ip != n; ++ip )
for ( size_type iq = ip + 1; iq != n; ++iq )
{
auto const tmp = std::abs( a[ip][iq] );
if ( current_max > tmp )
continue;
current_max = tmp;
p = ip;
q = iq;
}
// @if all non-diag value small, then break iteration with success
if ( current_max < eps )
{
std::copy( a.diag_begin(), a.diag_end(), o );
return i;
}
// a and V iterations
eigen_jacobi_private::rotate(a, V, p, q);
}//end for
#endif
// @just to kill warnings, should never reach here
return size_type( -1 );
}//eigen_jacobi
EllipseFit2<Real>::EllipseFit2 (int iQuantity, const Vector2<Real>* akPoint,
Vector2<Real>& rkU, Matrix2<Real>& rkR, Real afD[2], Real& rfError)
{
// Energy function is E : R^5 -> R where
// V = (V0,V1,V2,V3,V4)
// = (D[0],D[1],U.x,U,y,atan2(R[1][0],R[1][1])).
m_iQuantity = iQuantity;
m_akPoint = akPoint;
m_akTemp = WM4_NEW Vector2<Real>[iQuantity];
MinimizeN<Real> kMinimizer(5,Energy,8,8,32,this);
InitialGuess(iQuantity,akPoint,rkU,rkR,afD);
Real fAngle = Math<Real>::ACos(rkR[0][0]);
Real fE0 = afD[0]*Math<Real>::FAbs(rkR[0][0]) +
afD[1]*Math<Real>::FAbs(rkR[0][1]);
Real fE1 = afD[0]*Math<Real>::FAbs(rkR[1][0]) +
afD[1]*Math<Real>::FAbs(rkR[1][1]);
Real afV0[5] =
{
((Real)0.5)*afD[0],
((Real)0.5)*afD[1],
rkU.X() - fE0,
rkU.Y() - fE1,
(Real)0.0
};
Real afV1[5] =
{
((Real)2.0)*afD[0],
((Real)2.0)*afD[1],
rkU.X() + fE0,
rkU.Y() + fE1,
Math<Real>::PI
};
Real afVInitial[5] =
{
afD[0],
afD[1],
rkU.X(),
rkU.Y(),
fAngle
};
Real afVMin[5];
kMinimizer.GetMinimum(afV0,afV1,afVInitial,afVMin,rfError);
afD[0] = afVMin[0];
afD[1] = afVMin[1];
rkU.X() = afVMin[2];
rkU.Y() = afVMin[3];
rkR.FromAngle(afVMin[4]);
WM4_DELETE[] m_akTemp;
}
TEST(MatTypesTest, invert)
{
Matrix2 M(Matrix2::Line(4.0, 7.0), Matrix2::Line(2.0, 6.0));
Matrix2 Minv;
Matrix2 Mtest(Matrix2::Line(0.6,-0.7),
Matrix2::Line(-0.2,0.4));
invertMatrix(Minv, M);
EXPECT_EQ(Minv, Mtest);
EXPECT_EQ(M.inverted(), Mtest);
Minv.invert(M);
EXPECT_EQ(Minv, Mtest);
M.invert(M);
EXPECT_EQ(M, Mtest);
}
