void TransformMatrix2D::scale(const double sx, const double sy)
{
unitMatrix();
k_[0][0] = sx;
k_[1][1] = sy;
}
void TransformMatrix2D::translate(const double tx, const double ty)
{
unitMatrix();
k_[0][2] = tx;
k_[1][2] = ty;
}
void TransformMatrix2D::mirror(const bool x_axis, const bool y_axis)
{
unitMatrix();
k_[0][0] = y_axis ? -1.0 : 1.0;
k_[1][1] = x_axis ? -1.0 : 1.0;
}
dQuaternion::dQuaternion (const dMatrix &matrix)
{
enum QUAT_INDEX
{
X_INDEX=0,
Y_INDEX=1,
Z_INDEX=2
};
static QUAT_INDEX QIndex [] = {Y_INDEX, Z_INDEX, X_INDEX};
dFloat trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
dAssert (((matrix[0] * matrix[1]) % matrix[2]) > 0.0f);
if (trace > dFloat(0.0f)) {
trace = dSqrt (trace + dFloat(1.0f));
m_q0 = dFloat (0.5f) * trace;
trace = dFloat (0.5f) / trace;
m_q1 = (matrix[1][2] - matrix[2][1]) * trace;
m_q2 = (matrix[2][0] - matrix[0][2]) * trace;
m_q3 = (matrix[0][1] - matrix[1][0]) * trace;
} else {
QUAT_INDEX i = X_INDEX;
if (matrix[Y_INDEX][Y_INDEX] > matrix[X_INDEX][X_INDEX]) {
i = Y_INDEX;
}
if (matrix[Z_INDEX][Z_INDEX] > matrix[i][i]) {
i = Z_INDEX;
}
QUAT_INDEX j = QIndex [i];
QUAT_INDEX k = QIndex [j];
trace = dFloat(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k];
trace = dSqrt (trace);
dFloat* const ptr = &m_q1;
ptr[i] = dFloat (0.5f) * trace;
trace = dFloat (0.5f) / trace;
m_q0 = (matrix[j][k] - matrix[k][j]) * trace;
ptr[j] = (matrix[i][j] + matrix[j][i]) * trace;
ptr[k] = (matrix[i][k] + matrix[k][i]) * trace;
}
#if _DEBUG
dMatrix tmp (*this, matrix.m_posit);
dMatrix unitMatrix (tmp * matrix.Inverse());
for (int i = 0; i < 4; i ++) {
dFloat err = dAbs (unitMatrix[i][i] - dFloat(1.0f));
dAssert (err < dFloat (1.0e-3f));
}
dFloat err = dAbs (DotProduct(*this) - dFloat(1.0f));
dAssert (err < dFloat(1.0e-3f));
#endif
}
int main()
{
matrix m, n, r, t;
m = initmatrix(4,5);
printf("matrix m:\n");
printMatrix(&m);
n = initmatrix(4,4);
unitMatrix(&n);
printf("matrix n:\n");
printMatrix(&n);
r = initmatrix(6,6);
unitMatrix(&r);
printf("matrix r:\n");
printMatrix(&r);
t = initmatrix(6,6);
// t = initMatrix(6,6);
printf("enter row num: ");
int rown, coln, val;
scanf("%d", &rown);
printf("\nenter col num: ");
scanf("%d", &coln);
printf("\nenter value: ");
scanf("%d", &val);
if (rown >= 6 || coln >= 6)
{
printf("row or col number out of bounds!\n");
return 1;
}
t.data[rown][coln] = val;
//t.data[2][2] = 1;
printf("matrix t:\n");
printMatrix(&t);
}
dgQuaternion::dgQuaternion (const dgMatrix& matrix)
{
enum QUAT_INDEX
{
X_INDEX=0,
Y_INDEX=1,
Z_INDEX=2
};
static QUAT_INDEX QIndex [] = {Y_INDEX, Z_INDEX, X_INDEX};
dgFloat32 trace = matrix[0][0] + matrix[1][1] + matrix[2][2];
if (trace > dgFloat32(0.0f)) {
trace = dgSqrt (trace + dgFloat32(1.0f));
m_q0 = dgFloat32 (0.5f) * trace;
trace = dgFloat32 (0.5f) / trace;
m_q1 = (matrix[1][2] - matrix[2][1]) * trace;
m_q2 = (matrix[2][0] - matrix[0][2]) * trace;
m_q3 = (matrix[0][1] - matrix[1][0]) * trace;
} else {
QUAT_INDEX i = X_INDEX;
if (matrix[Y_INDEX][Y_INDEX] > matrix[X_INDEX][X_INDEX]) {
i = Y_INDEX;
}
if (matrix[Z_INDEX][Z_INDEX] > matrix[i][i]) {
i = Z_INDEX;
}
QUAT_INDEX j = QIndex [i];
QUAT_INDEX k = QIndex [j];
trace = dgFloat32(1.0f) + matrix[i][i] - matrix[j][j] - matrix[k][k];
trace = dgSqrt (trace);
dgFloat32* const ptr = &m_q1;
ptr[i] = dgFloat32 (0.5f) * trace;
trace = dgFloat32 (0.5f) / trace;
m_q0 = (matrix[j][k] - matrix[k][j]) * trace;
ptr[j] = (matrix[i][j] + matrix[j][i]) * trace;
ptr[k] = (matrix[i][k] + matrix[k][i]) * trace;
}
#ifdef _DEBUG
dgMatrix tmp (*this, matrix.m_posit);
dgMatrix unitMatrix (tmp * matrix.Inverse());
for (dgInt32 i = 0; i < 4; i ++) {
dgFloat32 err = dgAbsf (unitMatrix[i][i] - dgFloat32(1.0f));
dgAssert (err < dgFloat32 (1.0e-2f));
}
dgFloat32 err = dgAbsf (DotProduct(*this) - dgFloat32(1.0f));
dgAssert (err < dgFloat32(dgEPSILON * 100.0f));
#endif
}
void TransformMatrix2D::rotate(const double theta)
{
unitMatrix();
double c = Mathtools::COS(theta);
double s = Mathtools::SIN(theta);
k_[0][0] = c;
k_[0][1] = -s;
k_[1][0] = s;
k_[1][1] = c;
}
const Matrix rotation( const Vector& axis, const double angle )
{
const double cosa = cos(angle);
const double sina = sin(angle);
Vector ax = axis / sqrt( axis*axis );
Matrix op;
outer_product( ax, ax, op );
Matrix result = unitMatrix() * cosa
+ op * ( 1.0 - cosa )
+ PauliMatrix[0]*axis[0] * sina
+ PauliMatrix[1]*axis[1] * sina
+ PauliMatrix[2]*axis[2] * sina;
return result;
}
void SpinAdapted::operatorfunctions::TensorTraceElement(const SpinBlock *ablock, const Baseoperator<Matrix>& a, const SpinBlock *cblock, const StateInfo *cstateinfo, Baseoperator<Matrix>& c, Matrix& cel, int cq, int cqprime, double scale)
{
if (fabs(scale) < TINY) return;
assert(c.allowed(cq, cqprime));
int aq, aqprime, bq, bqprime, bstates;
const char conjC = (ablock == cblock->get_leftBlock()) ? 'n' : 't';
const std::vector<int> oldToNewI = cstateinfo->oldToNewState.at(cq);
const std::vector<int> oldToNewJ = cstateinfo->oldToNewState.at(cqprime);
const StateInfo* rS = cstateinfo->rightStateInfo, *lS = cstateinfo->leftStateInfo;
int rowstride =0, colstride = 0;
for (int oldi =0; oldi < oldToNewI.size(); oldi++) {
colstride = 0;
for (int oldj = 0; oldj < oldToNewJ.size(); oldj++)
{
if (conjC == 'n')
{
aq = cstateinfo->leftUnMapQuanta[ oldToNewI[oldi] ];
aqprime = cstateinfo->leftUnMapQuanta[ oldToNewJ[oldj] ];
bq = cstateinfo->rightUnMapQuanta[ oldToNewI[oldi] ];
bqprime = cstateinfo->rightUnMapQuanta[ oldToNewJ[oldj] ];
bstates = cstateinfo->rightStateInfo->getquantastates(bq);
}
else
{
aq = cstateinfo->rightUnMapQuanta[ oldToNewI[oldi] ];
aqprime = cstateinfo->rightUnMapQuanta[ oldToNewJ[oldj] ];
bq = cstateinfo->leftUnMapQuanta[ oldToNewI[oldi] ];
bqprime = cstateinfo->leftUnMapQuanta[ oldToNewJ[oldj] ];
bstates = cstateinfo->leftStateInfo->getquantastates(bq);
}
if (a.allowed(aq, aqprime) && (bq == bqprime))
{
DiagonalMatrix unitMatrix(bstates);
unitMatrix = 1.;
Matrix unity(bstates, bstates);
unity = unitMatrix;
if (conjC == 'n')
{
double scaleb = dmrginp.get_ninej()(lS->quanta[aqprime].get_s().getirrep() , rS->quanta[bqprime].get_s().getirrep(), cstateinfo->quanta[cqprime].get_s().getirrep(),
a.get_spin().getirrep(), 0, c.get_spin().getirrep(),
lS->quanta[aq].get_s().getirrep() , rS->quanta[bq].get_s().getirrep(), cstateinfo->quanta[cq].get_s().getirrep());
scaleb *= Symmetry::spatial_ninej(lS->quanta[aqprime].get_symm().getirrep() , rS->quanta[bqprime].get_symm().getirrep(), cstateinfo->quanta[cqprime].get_symm().getirrep(),
a.get_symm().getirrep(), 0, c.get_symm().getirrep(),
lS->quanta[aq].get_symm().getirrep() , rS->quanta[bq].get_symm().getirrep(), cstateinfo->quanta[cq].get_symm().getirrep());
MatrixTensorProduct (a.operator_element(aq, aqprime), a.conjugacy(), scale, unity, 'n', scaleb,
cel, rowstride, colstride);
}
else {
double scaleb = dmrginp.get_ninej()(lS->quanta[bqprime].get_s().getirrep(), rS->quanta[aqprime].get_s().getirrep() , cstateinfo->quanta[cqprime].get_s().getirrep(),
0, a.get_spin().getirrep(), c.get_spin().getirrep(),
lS->quanta[bq].get_s().getirrep(), rS->quanta[aq].get_s().getirrep() , cstateinfo->quanta[cq].get_s().getirrep());
scaleb *= Symmetry::spatial_ninej(lS->quanta[bqprime].get_symm().getirrep() , rS->quanta[aqprime].get_symm().getirrep(), cstateinfo->quanta[cqprime].get_symm().getirrep(),
0, a.get_symm().getirrep(), c.get_symm().getirrep(),
lS->quanta[bq].get_symm().getirrep() , rS->quanta[aq].get_symm().getirrep(), cstateinfo->quanta[cq].get_symm().getirrep());
if (a.get_fermion() && IsFermion (cstateinfo->leftStateInfo->quanta[bqprime]) ) scaleb *= -1.;
MatrixTensorProduct (unity, 'n', scaleb, a.operator_element(aq, aqprime), a.conjugacy(), scale,
cel, rowstride, colstride);
}
}
colstride += cstateinfo->unCollectedStateInfo->quantaStates[ oldToNewJ[oldj] ];
}
rowstride += cstateinfo->unCollectedStateInfo->quantaStates[ oldToNewI[oldi] ];
}
}
void
gaussianEstimator_Pred_decomp ( Matrix *xEst, Matrix *CEst, Matrix *U, Matrix *Cw, float *dt, Matrix *m_opt)
{
float r;
Matrix sizeMopt;
Matrix xn = zeroMatrix(3,1);
Matrix Cn = zeroMatrix(3,3);
Matrix xl = zeroMatrix(9,1);
Matrix Cl = zeroMatrix(9,9);
Matrix Cnl = zeroMatrix(3,9);
Matrix Cnl_T;
Matrix Cn_i;
Matrix CLN;
Matrix sizeCn;
Matrix Vec;
Matrix Val;
Matrix m1;
Matrix m;
Matrix x;
Matrix A;
Matrix Hi = zeroMatrix(12,9);
Matrix Cy = zeroMatrix(12, 12);
Matrix muy = zeroMatrix(12, 1);
Matrix zeros33 = zeroMatrix(3,3);
Matrix eye33 = unitMatrix(3,3);
Matrix Mat;
Matrix H;
Matrix gi = zeroMatrix(12,1);
Matrix Rot_vec = zeroMatrix(3,1);
Matrix mui;
Matrix muiy;
Matrix Ciy;
Matrix tmp;
Matrix tmp1;
Matrix tmp2;
Matrix tmp3;
Matrix tmp4;
Matrix tmp5;
Matrix tmp6;
Matrix tmp7;
Matrix tmp8;
Matrix tmpHi;
sizeMopt = sizeOfMatrix(*m_opt); //printf("%f\n",*dt);
float D = elem(sizeMopt,0,1)+1; //printf("%f\n", D);
freeMatrix(sizeMopt);
float w_opt = 1/D; //printf("%f\n", w_opt);
float Nx = 3;
float d = Nx*(D-1) + 1; //printf("%f\n", d);
float w = 1/d; //printf("%f\n", w);
//xn = xEst(4:6); % Rotation vector
appMatrix(xn, 0, 2, 0, 0, *xEst, 3, 5, 0, 0); //printMatrix(xn);system("PAUSE");
//Cn = CEst(4:6,4:6);
appMatrix(Cn, 0, 2, 0, 2, *CEst, 3, 5, 3, 5); //printMatrix(Cn);system("PAUSE");
//xl = [xEst(1:3) ; xEst(7:12)]; % Translation, angular velocity, linear velocity
appMatrix(xl, 0, 2, 0, 0, *xEst, 0, 2, 0, 0);
appMatrix(xl, 3, 8, 0, 0, *xEst, 6, 11, 0, 0); //printMatrix(xl);system("PAUSE");
//Cl = [CEst(1:3,1:3) CEst(1:3,7:12);
// CEst(7:12,1:3) CEst(7:12,7:12)] ;
appMatrix(Cl, 0, 2, 0, 2, *CEst, 0, 2, 0, 2);
appMatrix(Cl, 0, 2, 3, 8, *CEst, 0, 2, 6, 11);
appMatrix(Cl, 3, 8, 0, 2, *CEst, 6, 11, 0, 2);
appMatrix(Cl, 3, 8, 3, 8, *CEst, 6, 11, 6, 11); //printMatrix(Cl);system("PAUSE");
//Cnl = [CEst(4:6,1:3) CEst(4:6,7:12)];
appMatrix(Cnl, 0, 2, 0, 2, *CEst, 3, 5, 0, 2);
appMatrix(Cnl, 0, 2, 3, 8, *CEst, 3, 5, 6, 11); //printMatrix(Cnl);system("PAUSE");
//CLN = Cl - Cnl'*inv(Cn)*Cnl;
Cnl_T = transposeMatrix(Cnl);
// printMatrix(Cn);system("PAUSE");
Cn_i = invertCovMatrix(Cn); //printMatrix(Cn_i);system("PAUSE");
tmp = mulMatrix( Cnl_T, Cn_i);
tmp7 = mulMatrix(tmp, Cnl);
CLN = subMatrix ( Cl, tmp7); //printMatrix(CLN);system("PAUSE");
freeMatrix(tmp);
freeMatrix(tmp7);
// Eigenvectors, Eigenvalues
sizeCn = sizeOfMatrix(Cn);
int dimC = elem ( sizeCn, 0, 0 );
freeMatrix(sizeCn);
Vec = zeroMatrix(dimC, dimC);
Val = zeroMatrix(dimC, dimC);
eig ( &Cn, &Vec, &Val ); //printMatrix(Cn);printMatrix(Vec);printMatrix(Val);system("PAUSE");
// m1 = vec*sqrtf(val)
int i;
for ( i = 0; i < dimC; ++i )
setElem(Val, i, i, sqrtf(fabs(elem(Val, i,i))));
m1 = mulMatrix(Vec, Val); //printMatrix(m1);system("PAUSE");
// rotate & scale samples: m = m1*S
m = scaledSamplePoints(m1, *m_opt); //printMatrix(m);system("PAUSE");
// x = x*ones(1,d)
x = fillMatrix(xn, d);
// shift samples: m = m + x
tmp = addMatrix(m, x);
appMatrix(m, 0, m->height-1, 0, m->width-1, tmp, 0, tmp->height-1, 0, tmp->width-1 ); //printMatrix(m);system("PAUSE");
freeMatrix(tmp);
//A = [[eye(3,3),t*eye(3,3)];[zeros(3,3),eye(3,3)]];
A = unitMatrix(6,6);
setElem(A, 0, 3, *dt);
setElem(A, 1, 4, *dt);
setElem(A, 2, 5, *dt); //printMatrix(A);system("PAUSE");
for (i=0; i<d; i++)
{
//gi = [zeros(3,1); m(:,i); zeros(6,1)];
setElem(gi, 3, 0, elem(m, 0, i));
setElem(gi, 4, 0, elem(m, 1, i));
setElem(gi, 5, 0, elem(m, 2, i)); //printMatrix(gi);system("PAUSE");
//Rot_vec = m(:,i);
setElem(Rot_vec, 0, 0, elem(m, 0, i));
setElem(Rot_vec, 1, 0, elem(m, 1, i));
setElem(Rot_vec, 2, 0, elem(m, 2, i)); //printMatrix(Rot_vec);system("PAUSE");
//r = norm(Rot_vec);
r = sqrtf( powf((elem(Rot_vec,0,0)),2) + powf((elem(Rot_vec,1,0)),2) + powf((elem(Rot_vec,2,0)),2) ); //printf("%f\n",r);
H = zeroMatrix(3,3);
if (fmod(r, 2*pi) == 0)
{
Mat = unitMatrix(3,3);
}
else
{
// build skew symmetric Matrix
setElem(H, 0, 1, -elem(Rot_vec,2,0));
setElem(H, 0, 2, elem(Rot_vec,1,0));
setElem(H, 1, 0, elem(Rot_vec,2,0));
setElem(H, 1, 2, -elem(Rot_vec,0,0));
setElem(H, 2, 0, -elem(Rot_vec,1,0));
setElem(H, 2, 1, elem(Rot_vec,0,0)); //printMatrix(H);system("PAUSE");
// Bortz equation
// Mat = eye(3,3) + 0.5*H + (1- r*sin(r)/( 2*(1-cos(r))))/r^2*H*H;
// already declared Mat = unitMatrix(3,3);
tmp1 = mulScalarMatrix(0.5, H);
tmp4 = addMatrix( eye33 , tmp1 );
tmp2 = mulMatrix(H, H);
tmp3 = mulScalarMatrix( (1-(r*sin(r)/(2*(1-cos(r)))))/powf(r,2), tmp2);
Mat = addMatrix( tmp4, tmp3);
//printMatrix(Mat);system("PAUSE");
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
freeMatrix(tmp4);
}
//Hi = [[A(1:3,1:3) zeros(3,3) A(1:3,4:6)];
// [zeros(3,3), t*Mat, zeros(3,3)];
// [zeros(3,3), eye(3,3), zeros(3,3)];
// [A(4:6,1:3),zeros(3,3), A(4:6,4:6)]];
appMatrix( Hi, 0, 2, 0, 2, A, 0, 2, 0, 2 );
appMatrix( Hi, 0, 2, 3, 5, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 0, 2, 6, 8, A, 0, 2, 3, 5 );
appMatrix( Hi, 3, 5, 0, 2, zeros33, 0, 2, 0, 2 );
tmpHi = mulScalarMatrix(*dt, Mat);
appMatrix( Hi, 3, 5, 3, 5, tmpHi, 0, 2, 0, 2 );
freeMatrix(tmpHi);
appMatrix( Hi, 3, 5, 6, 8, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 6, 8, 0, 2, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 6, 8, 3, 5, eye33, 0, 2, 0, 2 );
appMatrix( Hi, 6, 8, 6, 8, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 9, 11, 0, 2, A, 3, 5, 0, 2 );
appMatrix( Hi, 9, 11, 3, 5, zeros33, 0, 2, 0, 2 );
appMatrix( Hi, 9, 11, 6, 8, A, 3, 5, 3, 5 ); //printMatrix(Hi);system("PAUSE");
// mui = xl + Cnl'*inv(Cn)*(m(:,i)-xn); //m(:,i) -> Rot_vec
tmp = mulMatrix(Cnl_T, Cn_i );
tmp1 = subMatrix(Rot_vec, xn);
tmp2 = mulMatrix(tmp, tmp1);
mui = addMatrix(xl, tmp2);
freeMatrix(tmp);
freeMatrix(tmp1);
freeMatrix(tmp2); //printMatrix(mui);system("PAUSE");
// muiy = gi + Hi * mui;
tmp = mulMatrix(Hi, mui);
muiy = addMatrix( gi, tmp); //printMatrix(muiy);system("PAUSE");
freeMatrix(tmp);
// Ciy = Hi *CLN *Hi';
tmp1 = mulMatrix(Hi, CLN);
tmp2 = transposeMatrix(Hi);
Ciy = mulMatrix( tmp1, tmp2); //printMatrix(Ciy);system("PAUSE");
freeMatrix(tmp1);
freeMatrix(tmp2);
// Cy = Cy + (w*Ciy + w_opt*muiy*muiy');
tmp3 = mulScalarMatrix(w, Ciy);
tmp1 = transposeMatrix(muiy);
tmp2 = mulMatrix(muiy, tmp1);
tmp4 = mulScalarMatrix( w_opt, tmp2 );
tmp5 = addMatrix( tmp3, tmp4 );
tmp6 = addMatrix( Cy, tmp5);
appMatrix(Cy,0,Cy->height-1,0,Cy->width-1,tmp6, 0,tmp6->height-1,0,tmp6->width-1); //printMatrix(Cy);system("PAUSE");
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp3);
freeMatrix(tmp4);
freeMatrix(tmp5);
freeMatrix(tmp6);
// muy = muy + w*muiy;
tmp = mulScalarMatrix( w, muiy );
tmp2 = addMatrix( muy, tmp );
appMatrix(muy,0,muy->height-1,0,muy->width-1, tmp2, 0, tmp2->height-1, 0, tmp2->width-1); //printMatrix(muy);system("PAUSE");
freeMatrix(tmp);
freeMatrix(tmp2);
freeMatrix(H);
freeMatrix(Mat);
freeMatrix(mui);//
freeMatrix(muiy);//
freeMatrix(Ciy);
}
appMatrix(*xEst, 0, 11, 0, 0, muy, 0, 11, 0, 0 ); //printMatrix(muy);system("PAUSE");
//CEst = Cy - muy*muy' * w_opt/w + Cw;
tmp1 = transposeMatrix(muy);
tmp2 = mulMatrix(muy, tmp1);
tmp5 = mulScalarMatrix( w_opt/w, tmp2 );
tmp6 = subMatrix(Cy, tmp5);
tmp8 = addMatrix( tmp6, *Cw); //printMatrix(*CEst);system("PAUSE");
appMatrix(*CEst,0,11,0,11, tmp8, 0,11,0,11 ); //printMatrix(tmp8);system("PAUSE");
freeMatrix(tmp1);
freeMatrix(tmp2);
freeMatrix(tmp5);
freeMatrix(tmp6);
freeMatrix(tmp8);
freeMatrix(muy);//
freeMatrix(zeros33);//
freeMatrix(Vec);
freeMatrix(Val);
freeMatrix(Cy);
freeMatrix(xn);
freeMatrix(Cn);
freeMatrix(xl);
freeMatrix(Cl);//
freeMatrix(Cnl);
freeMatrix(Cnl_T);
freeMatrix(Cn_i);
freeMatrix(CLN);//
freeMatrix(m1);
freeMatrix(m);//
freeMatrix(x);
freeMatrix(A);
freeMatrix(eye33);
freeMatrix(Hi);
freeMatrix(gi);
freeMatrix(Rot_vec);
} /* End gaussianPred_decomp */
TransformMatrix2D::TransformMatrix2D()
{
unitMatrix();
}
