void RotateAboutX(mat4x4* m, float angle)
{
mat4x4 rotation = IDENTITY_MATRIX;
float sine = (float)sin(angle);
float cosine = (float)cos(angle);
rotation.m[5] = cosine;
rotation.m[6] = sine;
rotation.m[9] = -sine;
rotation.m[10] = cosine;
memcpy(m->m, MultiplyMatrices(m, &rotation)->m, sizeof(m->m));
}
int MatrixExpv(int m, real * mtx, real t, real * v, real * w, int ideg_)
{
INTEGER ideg = ideg_;
INTEGER M = m;
INTEGER ldh = M;
INTEGER lwsp = 4*M*M+ideg+1;
real * wsp = (real*) malloc (sizeof(real) * lwsp);
INTEGER * ipiv = (INTEGER*) malloc (sizeof(INTEGER) * M);
INTEGER iexph = 0;
INTEGER ns = 0;
INTEGER flag = 0;
int mm = m*m;
real maxEntry = 0;
for(int i=0;i<mm; i++)
maxEntry = MAX(maxEntry, fabs(mtx[i]));
if (maxEntry != 0)
{
_xgpadm<sizeof(real)==sizeof(float)>::f(&ideg, &M, &t, mtx,
&ldh, wsp, &lwsp,
ipiv, &iexph, &ns, &flag);
//printf("iexph=%d ns=%d flag=%d\n", (int)iexph, (int)ns, (int)flag);
if (flag != 0)
{
printf("Error: xgpadm returned non-zero exit flag %d.\n", (int)flag);
return flag;
}
real * output = &wsp[iexph-1];
MultiplyMatrices(M, M, 1, output, v, w);
}
else
{
memcpy(w,v,sizeof(real)*M);
}
free(wsp);
return 0;
}
//----------------------------------------------------------------------
void LookAt(double *eye, double *target, double *upV, double *modelMatrix)
{
double forward[3], side[3], up[3];
double matrix2[16], resultMatrix[16];
//------------------
forward[0] = target[0] - eye[0];
forward[1] = target[1] - eye[1];
forward[2] = target[2] - eye[2];
NormalizeVector(forward);
//------------------
//Side = forward x up
ComputeNormalOfPlane(side, forward, upV);
NormalizeVector(side);
//------------------
//Recompute up as: up = side x forward
ComputeNormalOfPlane(up, side, forward);
//------------------
matrix2[0] = side[0];
matrix2[4] = side[1];
matrix2[8] = side[2];
matrix2[12] = 0.0;
//------------------
matrix2[1] = up[0];
matrix2[5] = up[1];
matrix2[9] = up[2];
matrix2[13] = 0.0;
//------------------
matrix2[2] = -forward[0];
matrix2[6] = -forward[1];
matrix2[10] = -forward[2];
matrix2[14] = 0.0;
//------------------
matrix2[3] = matrix2[7] = matrix2[11] = 0.0;
matrix2[15] = 1.0;
//------------------
MultiplyMatrices(resultMatrix, modelMatrix, matrix2);
Translate(resultMatrix, -eye[0], -eye[1], -eye[2]);
//------------------
memcpy(modelMatrix, resultMatrix, 16*sizeof(double));
}
void main()
{
int a[Rows][Cols];
int b[Rows][Cols];
int sum[Rows][Cols];
int product[Rows][Cols];
int i, j;
printf("Enter the values for the first matrix..\n");
for(i = 0; i < Rows; i++)
{
for(j = 0; j < Cols; j++)
{
scanf("%d", &a[i][j]);
}
}
printf("Enter the values for the second matrix..\n");
for(i = 0; i < Rows; i++)
{
for(j = 0; j < Cols; j++)
{
scanf("%d", &b[i][j]);
}
}
AddMatrices(a, b, sum);
MultiplyMatrices(a, b, product);
printf("The sum matrix is......\n");
PrintMatrix(sum);
printf("The product matrix is......\n");
PrintMatrix(product);
}
void SetModelMatrices()
{
Matrix matWorld, rotMatrix, transMatrix, scaleMatrix;
CreateRotationYMatrix(&rotMatrix, (timeGetTime()%2000)*(2.0f*M_PI)/2000.0f);
CreateTranslationMatrix(&transMatrix, 0.0f, ytranslation, 0.0f);
CreateScaleMatrix(&scaleMatrix, scaling, scaling, scaling);
MultiplyMatrices(&matWorld, &rotMatrix, &transMatrix);
//MultiplyMatrices(&matWorld, &matWorld, &scaleMatrix);
matWorld *= scaleMatrix;
Matrix finalmatrix = matWorld;
Vector3 vEyePt( 0.0f, 18.0f,-20.0f );
Vector3 vLookatPt( 0.0f, 0.0f, 0.0f );
Vector3 vUpVec( 0.0f, 1.0f, 0.0f );
Matrix matView;
CreateLookAtLHViewMatrix( &matView, &vEyePt, &vLookatPt, &vUpVec );
finalmatrix *= matView;
Matrix matProj;
CreatePerspectiveMatrix( &matProj, M_PI / 4, 1.0f, 1.0f, 100.0f );
finalmatrix *= matProj;
SetTransformMatrix(&finalmatrix);
}
void CCharShape::ScaleNode (size_t node_name, const TVector3& vec) {
TCharNode *node = GetNode(node_name);
if (node == NULL) return;
TMatrix matrix;
MakeIdentityMatrix (matrix);
MultiplyMatrices (node->trans, node->trans, matrix);
MakeIdentityMatrix (matrix);
MultiplyMatrices (node->invtrans, matrix, node->invtrans);
MakeScalingMatrix (matrix, vec.x, vec.y, vec.z);
MultiplyMatrices (node->trans, node->trans, matrix);
MakeScalingMatrix (matrix, 1.0 / vec.x, 1.0 / vec.y, 1.0 / vec.z);
MultiplyMatrices (node->invtrans, matrix, node->invtrans);
MakeIdentityMatrix (matrix);
MultiplyMatrices (node->trans, node->trans, matrix);
MakeIdentityMatrix (matrix);
MultiplyMatrices (node->invtrans, matrix, node->invtrans);
if (newActions && useActions) AddAction (node_name, 4, vec, 0);
}
real * PseudoInverseMatrix(int m, int n, real * mtx, real singularValueThreshold, int * rank_, real * output)
{
real * A = (real*) malloc (sizeof(real) * m * n);
memcpy(A, mtx, sizeof(real) * m * n);
bool transpose = (m > n);
if (transpose)
{
// swap m,n
InPlaceTransposeMatrix(m,n, A);
int buffer = m;
m = n;
n = buffer;
}
// now, we always have m <= n
char jobu = 'O';//overwrites A with U (left singular vectors)
char jobvt = 'S';//all rows returned in VT
int ldA = m;
int ldU = m;
INTEGER NB = 32; // optimal block size; could also call ilaenv
int lwork = NB*MAX(3*MIN( m, n)+MAX(m,n), 5*MIN(m,n)-4);
real * work = (real*) malloc (sizeof(real) * lwork);
if (!work)
{
printf("Error: failed to allocate workspace.\n");
throw 1;
}
//printf("Workspace size is: %G Mb .\n",1.0 * lwork * sizeof(int) / 1024 / 1024);
// allocate array for singular vectors
real * S = (real *) malloc (sizeof(real) * MIN(m,n));
if (!S)
{
printf("Error: failed to allocate singular vectors.\n");
free(work);
throw 2;
}
// allocate array for VT
int ldVT = MIN(m,n);
real * VT = (real *) malloc (sizeof(real) * ldVT * n);
if (!VT)
{
printf("Error: failed to allocate VT.\n");
free(S);
free(work);
throw 3;
}
INTEGER M = m;
INTEGER N = n;
INTEGER LDA = ldA;
INTEGER LDU = ldU;
INTEGER LDVT = ldVT;
INTEGER LWORK = lwork;
INTEGER INFO;
//printf("Calling LAPACK dgesvd routine...\n");fflush(NULL);
//SUBROUTINE SGESVD( JOBU, JOBVT, M, N, A, LDA, S, U, LDU, VT, LDVT, WORK, LWORK, INFO )
_xgesvd<sizeof(real)==sizeof(float)>::f
(&jobu, &jobvt, &M, &N, A, &LDA,
S, NULL, &LDU, VT,
&LDVT, work, &LWORK, &INFO);
if (INFO != 0)
{
int code = INFO;
printf("Error: SVD solver returned non-zero exit code: %d.\n", code);
free(VT);
free(S);
free(work);
throw 4;
}
free(work);
//printf ("Singular values:\n");fflush(NULL);
//for (int i=0; i< MIN(m,n); i++)
// printf("%f ",S[i]);
//printf ("\n");
// discard small singular values
int rank = MIN(m,n);
for(int i=0; i< MIN(m,n); i++)
{
if (S[i] / S[0] < singularValueThreshold)
{
rank = i;
break;
}
}
for(int i=0; i< rank; i++)
S[i] = ((real)1.0) / S[i];
for(int i=rank; i< MIN(m,n); i++)
S[i] = 0.0;
real * U = A;
memset(&U[ELT(m,0,rank)], 0, sizeof(real) * m * (n-rank));
InPlaceTransposeMatrix(m, m, U);
real * UT = U;
// UT is now m x m
InPlaceTransposeMatrix(m,n,VT);
real * V = VT;
// V is now n x m
// multiply A^{\dagger} = V * Sigma^{-1} * U^T
for(int j=0; j<m; j++) // over all columns
for(int i=0; i<n; i++) // over all rows
V[ELT(n, i, j)] *= S[j];
real * target = output;
if (target == NULL)
target = (real*) malloc (sizeof(real) * n * m);
// V * UT
// (n x m) * (m x m)
// output is n x m
MultiplyMatrices(n, m, m, V, UT, target);
if (transpose)
InPlaceTransposeMatrix(n, m, target);
free(A);
free(S);
free(VT);
if (rank_ != NULL)
*rank_ = rank;
return target;
}
void CCharShape::RefreshNode (size_t idx) {
if (idx >= numNodes) return;
TMatrix TempMatrix;
char caxis;
double angle;
TCharNode *node = Nodes[idx];
TCharAction *act = node->action;
if (act == NULL) return;
if (act->num < 1) return;
MakeIdentityMatrix (node->trans);
MakeIdentityMatrix (node->invtrans);
for (size_t i=0; i<act->num; i++) {
int type = act->type[i];
const TVector3& vec = act->vec[i];
double dval = act->dval[i];
switch (type) {
case 0:
MakeTranslationMatrix (TempMatrix, vec.x, vec.y, vec.z);
MultiplyMatrices (node->trans, node->trans, TempMatrix);
MakeTranslationMatrix (TempMatrix, -vec.x, -vec.y, -vec.z);
MultiplyMatrices (node->invtrans, TempMatrix, node->invtrans);
break;
case 1:
caxis = 'x';
angle = dval;
MakeRotationMatrix (TempMatrix, angle, caxis);
MultiplyMatrices (node->trans, node->trans, TempMatrix);
MakeRotationMatrix (TempMatrix, -angle, caxis);
MultiplyMatrices (node->invtrans, TempMatrix, node->invtrans);
break;
case 2:
caxis = 'y';
angle = dval;
MakeRotationMatrix (TempMatrix, angle, caxis);
MultiplyMatrices (node->trans, node->trans, TempMatrix);
MakeRotationMatrix (TempMatrix, -angle, caxis);
MultiplyMatrices (node->invtrans, TempMatrix, node->invtrans);
break;
case 3:
caxis = 'z';
angle = dval;
MakeRotationMatrix (TempMatrix, angle, caxis);
MultiplyMatrices (node->trans, node->trans, TempMatrix);
MakeRotationMatrix (TempMatrix, -angle, caxis);
MultiplyMatrices (node->invtrans, TempMatrix, node->invtrans);
break;
case 4:
MakeIdentityMatrix (TempMatrix);
MultiplyMatrices (node->trans, node->trans, TempMatrix);
MakeIdentityMatrix (TempMatrix);
MultiplyMatrices (node->invtrans, TempMatrix, node->invtrans);
MakeScalingMatrix (TempMatrix, vec.x, vec.y, vec.z);
MultiplyMatrices (node->trans, node->trans, TempMatrix);
MakeScalingMatrix (TempMatrix, 1.0 / vec.x, 1.0 / vec.y, 1.0 / vec.z);
MultiplyMatrices (node->invtrans, TempMatrix, node->invtrans);
MakeIdentityMatrix (TempMatrix);
MultiplyMatrices (node->trans, node->trans, TempMatrix);
MakeIdentityMatrix (TempMatrix);
MultiplyMatrices (node->invtrans, TempMatrix, node->invtrans);
break;
case 5:
VisibleNode (node->node_name, dval);
break;
default:
break;
}
}
}
