C++ MatScale函数代码示例

PetscErrorCode StokesSetupApproxSchur(Stokes *s)
{
  Vec            diag;
  PetscErrorCode ierr;

  PetscFunctionBeginUser;
  /* Schur complement approximation: myS = A11 - A10 diag(A00)^(-1) A01 */
  /* note: A11 is zero */
  /* note: in real life this matrix would be build directly, */
  /* i.e. without MatMatMult */

  /* inverse of diagonal of A00 */
  ierr = VecCreate(PETSC_COMM_WORLD,&diag);CHKERRQ(ierr);
  ierr = VecSetSizes(diag,PETSC_DECIDE,2*s->nx*s->ny);CHKERRQ(ierr);
  ierr = VecSetType(diag,VECMPI);CHKERRQ(ierr);
  ierr = MatGetDiagonal(s->subA[0],diag);
  ierr = VecReciprocal(diag);

  /* compute: - A10 diag(A00)^(-1) A01 */
  ierr = MatDiagonalScale(s->subA[1],diag,NULL); /* (*warning* overwrites subA[1]) */
  ierr = MatMatMult(s->subA[2],s->subA[1],MAT_INITIAL_MATRIX,PETSC_DEFAULT,&s->myS);CHKERRQ(ierr);
  ierr = MatScale(s->myS,-1.0);CHKERRQ(ierr);

  /* restore A10 */
  ierr = MatGetDiagonal(s->subA[0],diag);
  ierr = MatDiagonalScale(s->subA[1],diag,NULL);
  ierr = VecDestroy(&diag);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

// Create a translation matrix
static void MatTranslate(Matrix44 &dest,sF32 tx,sF32 ty,sF32 tz)
{
  MatScale(dest,1.0f,1.0f,1.0f);
  dest[3][0] = tx;
  dest[3][1] = ty;
  dest[3][2] = tz;
}

PetscErrorCode TSPrecond_Sundials(realtype tn,N_Vector y,N_Vector fy,booleantype jok,booleantype *jcurPtr,
                                  realtype _gamma,void *P_data,N_Vector vtemp1,N_Vector vtemp2,N_Vector vtemp3)
{
  TS             ts     = (TS) P_data;
  TS_Sundials    *cvode = (TS_Sundials*)ts->data;
  PC             pc;
  PetscErrorCode ierr;
  Mat            J,P;
  Vec            yy  = cvode->w1,yydot = cvode->ydot;
  PetscReal      gm  = (PetscReal)_gamma;
  MatStructure   str = DIFFERENT_NONZERO_PATTERN;
  PetscScalar    *y_data;

  PetscFunctionBegin;
  ierr   = TSGetIJacobian(ts,&J,&P,NULL,NULL);CHKERRQ(ierr);
  y_data = (PetscScalar*) N_VGetArrayPointer(y);
  ierr   = VecPlaceArray(yy,y_data);CHKERRQ(ierr);
  ierr   = VecZeroEntries(yydot);CHKERRQ(ierr); /* The Jacobian is independent of Ydot for ODE which is all that CVode works for */
  /* compute the shifted Jacobian   (1/gm)*I + Jrest */
  ierr     = TSComputeIJacobian(ts,ts->ptime,yy,yydot,1/gm,&J,&P,&str,PETSC_FALSE);CHKERRQ(ierr);
  ierr     = VecResetArray(yy);CHKERRQ(ierr);
  ierr     = MatScale(P,gm);CHKERRQ(ierr); /* turn into I-gm*Jrest, J is not used by Sundials  */
  *jcurPtr = TRUE;
  ierr     = TSSundialsGetPC(ts,&pc);CHKERRQ(ierr);
  ierr     = PCSetOperators(pc,J,P,str);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

void Field_solver::construct_equation_matrix_in_full_domain( Mat *A,
							     int nx, int ny, int nz,
							     double dx, double dy, double dz,
							     PetscInt nlocal, PetscInt rstart, PetscInt rend )
{
    PetscErrorCode ierr;
    Mat d2dy2, d2dz2;
    int nrow = ( nx - 2 ) * ( ny - 2 ) * ( nz - 2 );
    int ncol = nrow;
    PetscInt nonzero_per_row = 7; // approx

    construct_d2dx2_in_3d( A, nx, ny, nz, rstart, rend );
    ierr = MatScale( *A, dy * dy * dz * dz ); CHKERRXX( ierr );
    
    alloc_petsc_matrix( &d2dy2, nlocal, nlocal, nrow, ncol, nonzero_per_row );
    construct_d2dy2_in_3d( &d2dy2, nx, ny, nz, rstart, rend );
    ierr = MatAXPY( *A, dx * dx * dz * dz, d2dy2, DIFFERENT_NONZERO_PATTERN ); CHKERRXX( ierr );
    ierr = MatDestroy( &d2dy2 ); CHKERRXX( ierr );

    alloc_petsc_matrix( &d2dz2, nlocal, nlocal, nrow, ncol, nonzero_per_row );
    construct_d2dz2_in_3d( &d2dz2, nx, ny, nz, rstart, rend );
    ierr = MatAXPY( *A, dx * dx * dy * dy, d2dz2, DIFFERENT_NONZERO_PATTERN ); CHKERRXX( ierr );
    ierr = MatDestroy( &d2dz2 ); CHKERRXX( ierr );

    return;
}

PetscErrorCode CalcMat(FEMInf fem, int L, Mat *H, Mat *S) {

  PetscErrorCode ierr;
  char label[10]; sprintf(label, "L+%d", L);
  PrintTimeStamp(fem->comm, label, NULL);

  FEMInfCreateMat(fem, 1, H);
  FEMInfCreateMat(fem, 1, S);

  PetscBool s_is_id; FEMInfGetOverlapIsId(fem, &s_is_id);
  if(s_is_id)
    S = NULL;
  else {
    ierr = FEMInfSR1Mat(fem, *S); CHKERRQ(ierr); CHKERRQ(ierr);
  }
  
  ierr = FEMInfD2R1Mat(fem, *H); CHKERRQ(ierr);
  MatScale(*H, -0.5);

  if(L != 0) {
    Mat A;
    FEMInfCreateMat(fem, 1, &A);
    ierr = FEMInfR2invR1Mat(fem, A); CHKERRQ(ierr);
    MatAXPY(*H, 0.5*L*(L+1), A, DIFFERENT_NONZERO_PATTERN);
  }

  Mat V;
  FEMInfCreateMat(fem, 1, &V);
  FEMInfENR1Mat(fem, 0, 0.0, V);
  MatAXPY(*H, -1.0, V, DIFFERENT_NONZERO_PATTERN);

  return 0;
}

static PetscErrorCode CheckMatrices(Mat A,Mat B,Vec left,Vec right,Vec X,Vec Y,Vec X1,Vec Y1)
{
  PetscErrorCode ierr;
  Vec            *ltmp,*rtmp;

  PetscFunctionBegin;
  ierr = VecDuplicateVecs(right,2,&rtmp);CHKERRQ(ierr);
  ierr = VecDuplicateVecs(left,2,&ltmp);CHKERRQ(ierr);
  ierr = MatScale(A,PETSC_PI);CHKERRQ(ierr);
  ierr = MatScale(B,PETSC_PI);CHKERRQ(ierr);
  ierr = MatDiagonalScale(A,left,right);CHKERRQ(ierr);
  ierr = MatDiagonalScale(B,left,right);CHKERRQ(ierr);

  ierr = MatMult(A,X,ltmp[0]);CHKERRQ(ierr);
  ierr = MatMult(B,X,ltmp[1]);CHKERRQ(ierr);
  ierr = Compare2(ltmp,"MatMult");CHKERRQ(ierr);

  ierr = MatMultTranspose(A,Y,rtmp[0]);CHKERRQ(ierr);
  ierr = MatMultTranspose(B,Y,rtmp[1]);CHKERRQ(ierr);
  ierr = Compare2(rtmp,"MatMultTranspose");CHKERRQ(ierr);

  ierr = VecCopy(Y1,ltmp[0]);CHKERRQ(ierr);
  ierr = VecCopy(Y1,ltmp[1]);CHKERRQ(ierr);
  ierr = MatMultAdd(A,X,ltmp[0],ltmp[0]);CHKERRQ(ierr);
  ierr = MatMultAdd(B,X,ltmp[1],ltmp[1]);CHKERRQ(ierr);
  ierr = Compare2(ltmp,"MatMultAdd v2==v3");CHKERRQ(ierr);

  ierr = MatMultAdd(A,X,Y1,ltmp[0]);CHKERRQ(ierr);
  ierr = MatMultAdd(B,X,Y1,ltmp[1]);CHKERRQ(ierr);
  ierr = Compare2(ltmp,"MatMultAdd v2!=v3");CHKERRQ(ierr);

  ierr = VecCopy(X1,rtmp[0]);CHKERRQ(ierr);
  ierr = VecCopy(X1,rtmp[1]);CHKERRQ(ierr);
  ierr = MatMultTransposeAdd(A,Y,rtmp[0],rtmp[0]);CHKERRQ(ierr);
  ierr = MatMultTransposeAdd(B,Y,rtmp[1],rtmp[1]);CHKERRQ(ierr);
  ierr = Compare2(rtmp,"MatMultTransposeAdd v2==v3");CHKERRQ(ierr);

  ierr = MatMultTransposeAdd(A,Y,X1,rtmp[0]);CHKERRQ(ierr);
  ierr = MatMultTransposeAdd(B,Y,X1,rtmp[1]);CHKERRQ(ierr);
  ierr = Compare2(rtmp,"MatMultTransposeAdd v2!=v3");CHKERRQ(ierr);

  ierr = VecDestroyVecs(2,&ltmp);CHKERRQ(ierr);
  ierr = VecDestroyVecs(2,&rtmp);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

PetscErrorCode NEPSolve_Interpol(NEP nep)
{
  PetscErrorCode ierr;
  NEP_INTERPOL   *ctx = (NEP_INTERPOL*)nep->data;
  Mat            *A;   /*T=nep->function,Tp=nep->jacobian;*/
  PetscScalar    *x,*fx,t;
  PetscReal      *cs,a,b,s;
  PetscInt       i,j,k,deg=ctx->deg;

  PetscFunctionBegin;
  ierr = PetscMalloc4(deg+1,&A,(deg+1)*(deg+1),&cs,deg+1,&x,(deg+1)*nep->nt,&fx);CHKERRQ(ierr);
  ierr = RGIntervalGetEndpoints(nep->rg,&a,&b,NULL,NULL);CHKERRQ(ierr);
  ierr = ChebyshevNodes(deg,a,b,x,cs);CHKERRQ(ierr);
  for (j=0;j<nep->nt;j++) {
    for (i=0;i<=deg;i++) {
      ierr = FNEvaluateFunction(nep->f[j],x[i],&fx[i+j*(deg+1)]);CHKERRQ(ierr);
    }
  }

  /* Polynomial coefficients */
  for (k=0;k<=deg;k++) {
    ierr = MatDuplicate(nep->A[0],MAT_COPY_VALUES,&A[k]);CHKERRQ(ierr);
    t = 0.0;
    for (i=0;i<deg+1;i++) t += fx[i]*cs[i*(deg+1)+k];
    t *= 2.0/(deg+1); 
    if (k==0) t /= 2.0;
    ierr = MatScale(A[k],t);CHKERRQ(ierr);
    for (j=1;j<nep->nt;j++) {
      t = 0.0;
      for (i=0;i<deg+1;i++) t += fx[i+j*(deg+1)]*cs[i*(deg+1)+k];
      t *= 2.0/(deg+1); 
      if (k==0) t /= 2.0;
      ierr = MatAXPY(A[k],t,nep->A[j],SUBSET_NONZERO_PATTERN);CHKERRQ(ierr);
    }
  }

  ierr = PEPSetOperators(ctx->pep,deg+1,A);CHKERRQ(ierr);
  for (k=0;k<=deg;k++) {
    ierr = MatDestroy(&A[k]);CHKERRQ(ierr);
  }
  ierr = PetscFree4(A,cs,x,fx);CHKERRQ(ierr);

  /* Solve polynomial eigenproblem */
  ierr = PEPSolve(ctx->pep);CHKERRQ(ierr);
  ierr = PEPGetConverged(ctx->pep,&nep->nconv);CHKERRQ(ierr);
  ierr = PEPGetIterationNumber(ctx->pep,&nep->its);CHKERRQ(ierr);
  ierr = PEPGetConvergedReason(ctx->pep,(PEPConvergedReason*)&nep->reason);CHKERRQ(ierr);
  s = 2.0/(b-a);
  for (i=0;i<nep->nconv;i++) {
    ierr = PEPGetEigenpair(ctx->pep,i,&nep->eigr[i],&nep->eigi[i],NULL,NULL);CHKERRQ(ierr);
    nep->eigr[i] /= s;
    nep->eigr[i] += (a+b)/2.0;
    nep->eigi[i] /= s;
  }
  nep->state = NEP_STATE_EIGENVECTORS;
  PetscFunctionReturn(0);
}

PetscErrorCode MatScale_IS(Mat A,PetscScalar a)
{
  Mat_IS         *is = (Mat_IS*)A->data;
  PetscErrorCode ierr;

  PetscFunctionBegin;
  ierr = MatScale(is->A,a);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

PetscErrorCode MatScale_SMF(Mat mat, PetscReal a)
{
  PetscErrorCode   ierr;
  MatSubMatFreeCtx ctx;

  PetscFunctionBegin;
  ierr = MatShellGetContext(mat,(void **)&ctx);CHKERRQ(ierr);
  ierr = MatScale(ctx->A,a);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

// Create a z-axis rotation matrix
static void MatRotateZ(Matrix44 &dest,sF32 angle)
{
  sF32 s = sFSin(angle);
  sF32 c = sFCos(angle);

  MatScale(dest,1.0f,1.0f,1.0f);
  dest[0][0] = c;
  dest[0][1] = s;
  dest[1][0] = -s;
  dest[1][1] = c;
}

PetscErrorCode StokesSetupMatBlock10(Stokes *s)
{
  PetscErrorCode ierr;

  PetscFunctionBeginUser;
  /* A[2] is minus transpose of A[1] */
  ierr = MatTranspose(s->subA[1], MAT_INITIAL_MATRIX, &s->subA[2]);CHKERRQ(ierr);
  ierr = MatScale(s->subA[2], -1.0);CHKERRQ(ierr);
  ierr = MatSetOptionsPrefix(s->subA[2], "a10_");CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

int main(int argc,char **args)
{
  const PetscScalar xvals[] = {11,13},yvals[] = {17,19};
  const PetscInt    inds[]  = {0,1};
  PetscScalar       avals[] = {2,3,5,7};
  Mat               S1,S2;
  Vec               X,Y;
  User              user;
  PetscErrorCode    ierr;

  ierr = PetscInitialize(&argc,&args,(char*)0,help);if (ierr) return ierr;

  ierr = PetscNew(&user);CHKERRQ(ierr);
  ierr = MatCreateSeqAIJ(PETSC_COMM_WORLD,2,2,2,NULL,&user->A);CHKERRQ(ierr);
  ierr = MatSetUp(user->A);CHKERRQ(ierr);
  ierr = MatSetValues(user->A,2,inds,2,inds,avals,INSERT_VALUES);CHKERRQ(ierr);
  ierr = MatAssemblyBegin(user->A,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
  ierr = MatAssemblyEnd(user->A,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_WORLD,2,&X);CHKERRQ(ierr);
  ierr = VecSetValues(X,2,inds,xvals,INSERT_VALUES);CHKERRQ(ierr);
  ierr = VecAssemblyBegin(X);CHKERRQ(ierr);
  ierr = VecAssemblyEnd(X);CHKERRQ(ierr);
  ierr = VecDuplicate(X,&Y);CHKERRQ(ierr);
  ierr = VecSetValues(Y,2,inds,yvals,INSERT_VALUES);CHKERRQ(ierr);
  ierr = VecAssemblyBegin(Y);CHKERRQ(ierr);
  ierr = VecAssemblyEnd(Y);CHKERRQ(ierr);

  ierr = MatCreateShell(PETSC_COMM_WORLD,2,2,2,2,user,&S1);CHKERRQ(ierr);
  ierr = MatSetUp(S1);CHKERRQ(ierr);
  ierr = MatShellSetOperation(S1,MATOP_MULT,(void (*)(void))MatMult_User);CHKERRQ(ierr);
  ierr = MatShellSetOperation(S1,MATOP_COPY,(void (*)(void))MatCopy_User);CHKERRQ(ierr);
  ierr = MatShellSetOperation(S1,MATOP_DESTROY,(void (*)(void))MatDestroy_User);CHKERRQ(ierr);
  ierr = MatCreateShell(PETSC_COMM_WORLD,2,2,2,2,NULL,&S2);CHKERRQ(ierr);
  ierr = MatSetUp(S2);CHKERRQ(ierr);
  ierr = MatShellSetOperation(S2,MATOP_MULT,(void (*)(void))MatMult_User);CHKERRQ(ierr);
  ierr = MatShellSetOperation(S2,MATOP_COPY,(void (*)(void))MatCopy_User);CHKERRQ(ierr);
  ierr = MatShellSetOperation(S2,MATOP_DESTROY,(void (*)(void))MatDestroy_User);CHKERRQ(ierr);

  ierr = MatScale(S1,31);CHKERRQ(ierr);
  ierr = MatShift(S1,37);CHKERRQ(ierr);
  ierr = MatDiagonalScale(S1,X,Y);CHKERRQ(ierr);
  ierr = MatCopy(S1,S2,SAME_NONZERO_PATTERN);CHKERRQ(ierr);
  ierr = MatMult(S1,X,Y);CHKERRQ(ierr);
  ierr = VecView(Y,PETSC_VIEWER_STDOUT_WORLD);CHKERRQ(ierr);
  ierr = MatMult(S2,X,Y);CHKERRQ(ierr);
  ierr = VecView(Y,PETSC_VIEWER_STDOUT_WORLD);CHKERRQ(ierr);

  ierr = MatDestroy(&S1);CHKERRQ(ierr);
  ierr = MatDestroy(&S2);CHKERRQ(ierr);
  ierr = VecDestroy(&X);CHKERRQ(ierr);
  ierr = VecDestroy(&Y);CHKERRQ(ierr);
  ierr = PetscFinalize();
  return ierr;
}

int testSlaterPotWithECS() {
  PrintTimeStamp(PETSC_COMM_SELF, "ECS", NULL);

  MPI_Comm comm = PETSC_COMM_SELF;
  BPS bps; BPSCreate(comm, &bps); BPSSetLine(bps, 100.0, 101);
  CScaling scaler; CScalingCreate(comm, &scaler); 
  CScalingSetSharpECS(scaler, 60.0, 20.0*M_PI/180.0);

  int order = 5;
  BSS bss; BSSCreate(comm, &bss); BSSSetKnots(bss, order, bps);
  BSSSetCScaling(bss, scaler);   BSSSetUp(bss);
  Pot slater; PotCreate(comm, &slater); PotSetSlater(slater, 7.5, 2, 1.0);

  if(getenv("SHOW_DEBUG"))
    BSSView(bss, PETSC_VIEWER_STDOUT_SELF);

  Mat H; BSSCreateR1Mat(bss, &H); 
  Mat V; BSSCreateR1Mat(bss, &V); BSSPotR1Mat(bss, slater, V);
  Mat S; BSSCreateR1Mat(bss, &S); BSSSR1Mat(bss, S);

  BSSD2R1Mat(bss, H);
  MatScale(H, -0.5);
  MatAXPY(H, 1.0, V, DIFFERENT_NONZERO_PATTERN);

  EEPS eps; EEPSCreate(comm, &eps);
  EEPSSetOperators(eps, H, S);
  EEPSSetTarget(eps, 3.4);
  EPSSetDimensions(eps->eps, 10, PETSC_DEFAULT, PETSC_DEFAULT);
  EPSSetTolerances(eps->eps, PETSC_DEFAULT, 1000);
  //  EPSSetType(eps, EPSARNOLDI);

  EEPSSolve(eps);

  PetscInt nconv;
  PetscScalar kr;
  EPSGetConverged(eps->eps, &nconv);
  
  ASSERT_TRUE(nconv > 0);
  if(getenv("SHOW_DEBUG"))
    for(int i = 0; i < nconv; i++) {
      EPSGetEigenpair(eps->eps, i, &kr, NULL, NULL, NULL);
      PetscPrintf(comm, "%f, %f\n", PetscRealPart(kr), PetscImaginaryPart(kr));
    }

  EPSGetEigenpair(eps->eps, 0, &kr, NULL, NULL, NULL);

  PFDestroy(&slater); BSSDestroy(&bss);  EEPSDestroy(&eps);
  MatDestroy(&H); MatDestroy(&V); MatDestroy(&S);
  
  //  ASSERT_DOUBLE_NEAR(-0.0127745, PetscImaginaryPart(kr), pow(10.0, -4.0));
  //  ASSERT_DOUBLE_NEAR(3.4263903, PetscRealPart(kr), pow(10.0, -4.0));  
  return 0;
}

int main(int argc,char **argv)
{
  Mat            A,B;
  MatScalar      a[1],alpha;
  PetscMPIInt    size,rank;
  PetscInt       m,n,i,col, prid;
  PetscErrorCode ierr;

  PetscInitialize(&argc,&argv,(char *)0,help);
  ierr = MPI_Comm_size(PETSC_COMM_WORLD,&size);CHKERRQ(ierr);
  ierr = MPI_Comm_rank(PETSC_COMM_WORLD,&rank);CHKERRQ(ierr);
  prid = size;
  ierr = PetscOptionsGetInt(PETSC_NULL,"-prid",&prid,PETSC_NULL);CHKERRQ(ierr);

  m = n = 10*size;
  ierr = MatCreate(PETSC_COMM_SELF,&A);CHKERRQ(ierr);
  ierr = MatSetSizes(A,PETSC_DETERMINE,PETSC_DETERMINE,m,n);CHKERRQ(ierr);
  ierr = MatSetType(A,MATSEQAIJ);CHKERRQ(ierr);
  ierr = MatSetUp(A);CHKERRQ(ierr);

  a[0] = rank+1;
  for (i=0; i<m-rank; i++){
    col = i+rank;
    ierr = MatSetValues(A,1,&i,1,&col,a,INSERT_VALUES);CHKERRQ(ierr);
  }
  ierr = MatAssemblyBegin(A,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
  ierr = MatAssemblyEnd(A,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);

  if (rank == prid){
    ierr = PetscPrintf(PETSC_COMM_SELF,"[%d] A: \n",rank);
    ierr = MatView(A,PETSC_VIEWER_STDOUT_SELF);CHKERRQ(ierr);
  }

  /* Test MatCreateMPIAIJSumSeqAIJ */
  ierr = MatCreateMPIAIJSumSeqAIJ(PETSC_COMM_WORLD,A,PETSC_DECIDE,PETSC_DECIDE,MAT_INITIAL_MATRIX,&B);CHKERRQ(ierr);

  /* Test MAT_REUSE_MATRIX */
  alpha = 0.1;
  for (i=0; i<3; i++){
    ierr = MatScale(A,alpha);CHKERRQ(ierr);
    ierr = MatCreateMPIAIJSumSeqAIJ(PETSC_COMM_WORLD,A,PETSC_DECIDE,PETSC_DECIDE,MAT_REUSE_MATRIX,&B);CHKERRQ(ierr);
  }
  ierr = MatView(B, PETSC_VIEWER_STDOUT_WORLD);CHKERRQ(ierr);
  ierr = MatDestroy(&B);CHKERRQ(ierr);
  ierr = MatDestroy(&A);CHKERRQ(ierr);

  PetscFinalize();
  return(0);
}

static void make_transform_matrix(
		int transform_order, int rotate_order,
		double tx, double ty, double tz,
		double rx, double ry, double rz,
		double sx, double sy, double sz,
		double *transform)
{
	int i;
	double T[16], R[16], S[16], RX[16], RY[16], RZ[16];
	double *queue[3];

	MatTranslate(T, tx, ty, tz);
	MatRotateX(RX, rx);
	MatRotateY(RY, ry);
	MatRotateZ(RZ, rz);
	MatScale(S, sx, sy, sz);

	switch (rotate_order) {
	case ORDER_XYZ: VEC3_SET(queue, RX, RY, RZ); break;
	case ORDER_XZY: VEC3_SET(queue, RX, RZ, RY); break;
	case ORDER_YXZ: VEC3_SET(queue, RY, RX, RZ); break;
	case ORDER_YZX: VEC3_SET(queue, RY, RZ, RX); break;
	case ORDER_ZXY: VEC3_SET(queue, RZ, RX, RY); break;
	case ORDER_ZYX: VEC3_SET(queue, RZ, RY, RX); break;
	default:
		assert(!"invalid rotate order");
		break;
	}

	MatIdentity(R);
	for (i = 0; i < 3; i++)
		MatMultiply(R, queue[i], R);

	switch (transform_order) {
	case ORDER_SRT: VEC3_SET(queue, S, R, T); break;
	case ORDER_STR: VEC3_SET(queue, S, T, R); break;
	case ORDER_RST: VEC3_SET(queue, R, S, T); break;
	case ORDER_RTS: VEC3_SET(queue, R, T, S); break;
	case ORDER_TRS: VEC3_SET(queue, T, R, S); break;
	case ORDER_TSR: VEC3_SET(queue, T, S, R); break;
	default:
		assert(!"invalid transform order order");
		break;
	}

	MatIdentity(transform);
	for (i = 0; i < 3; i++)
		MatMultiply(transform, queue[i], transform);
}