Code: Select all
// Matrix algebra / arithmetics
// vers. 0.92t (HaWe+jch+splrc version)
#define MatrixMulS(_Out, _A, _s) asm { mul _Out, _A, _s }
#define MatrixDivS(_Out, _A, _s) asm { div _Out, _A, _s }
#define MatrixAdd(_Out, _A, _B) asm { add _Out, _A, _B }
#define MatrixSub(_Out, _A, _B) asm { sub _Out, _A, _B }
#define ArrayIndex(_out, _A, _i) asm { index _out, _A, _i }
#define ArrayReplace(_A, _i, _new) asm { replace _A, _A, _i, _new }
#define BranchTest(_cmp, _lbl, _v1) asm{ brtst _cmp, _lbl, _v1 }
#define BranchComp(_cmp, _lbl, _v1, _v2) asm{ brcmp _cmp, _lbl, _v1, _v2 }
#define ArrayInit2D(array, tmp, init_val, dimx, dimy) { \
ArrayInit(tmp, init_val, dimy); \
ArrayInit(array, tmp, dimx); \
ArrayInit(tmp,0,0); \
}
void MatrixTransp(float A[][], float &C[][], int R, int S) {
float tmp[], arr_temp[], val_temp;
int s, r;
ArrayInit(tmp, 0, R);
ArrayInit(C, tmp, S);
s = S;
lbl_Trans_start_s:
{
s--;
r = R;
lbl_Trans_start_r:
{
r--;
ArrayIndex(arr_temp, A, r);
ArrayIndex(val_temp, arr_temp, s);
ArrayReplace(tmp, r, val_temp);
}
BranchTest(GT, lbl_Trans_start_r, r);
ArrayReplace(C, s, tmp);
}
BranchTest(GT, lbl_Trans_start_s, s);
}
// N x M Matrix * M x K Matrix, OLD version
void MatrixMatrixMult_OLD(float A[][], float B[][], float &C[][], int N, int M, int K){
int i, j, s;
for (i=0; i<N; ++i) {
for (j=0; j<K; ++j) {
C[i][j]=0;
for (s=0; s<M; ++s) {
C[i][j]=C[i][j] + A[i][s]*B[s][j];
}
}
}
}
// N x M Matrix * M x K Matrix, NEW version
void MatrixMatrixMult(float A[][], float B[][], float &C[][], int N, int M, int K){
float arr_temp[];
//Set size of C
ArrayInit( arr_temp, 0, K );
ArrayInit( C, arr_temp, N );
lbl_Mult_start_i: N--; //Loop start
//Speed up arrays
float A_i[];
float C_i[];
ArrayIndex(A_i, A, N);
ArrayIndex(C_i, C, N);
int j = K;
lbl_Mult_start_j: j--; //Second loop start
float sum = 0;
int s = M;
lbl_Mult_start_s: s--; //Third loop start
//sum += A_i[s]*B[s][j];
float temp, sum_temp;
ArrayIndex(sum_temp, A_i, s);
ArrayIndex(arr_temp, B, s);
ArrayIndex(temp, arr_temp, j);
sum_temp *= temp;
sum += sum_temp;
asm { brtst GT, lbl_Mult_start_s, s }
ArrayReplace(C_i, j, sum);
asm { brtst GT, lbl_Mult_start_j, j }
ArrayReplace(C, N, C_i);
asm { brtst GT, lbl_Mult_start_i, N }
}
void MatrixRotate2DMath(float A[][], float &C[][], float angleDeg) {
float tmp[], x, B[][];
ArrayInit2D(C, tmp, 0, 2, 2);
x=cosd(angleDeg);
ArrayInit2D(B, tmp, x, 2, 2);
x=sind(angleDeg);
B[0][1]=-x;
B[1][0]= x;
MatrixMatrixMult(B, A, C, 2, 2, 2);
}
#define MatrixRotate2DGeo(A,C, d) MatrixRotate2DMath(A,C,-d)
float MatrixDet2x2(float A[][]) { // Determinante 1x1, 2x2, 3x3
float r0[], r1[], r2[], v00, v01, v02, v10, v11, v12, v20, v21, v22;
r0 = A[0];
r1 = A[1];
return ( r0[0]*r1[1]- r0[1]*r1[0] );
}
float MatrixDet3x3(float A[][]) { // Determinante 3x3
float r0[], r1[], r2[], v00, v01, v02, v10, v11, v12, v20, v21, v22;
r0 = A[0];
r1 = A[1];
r2 = A[2];
v00 = r0[0]; v01 = r0[1]; v02 = r0[2];
v10 = r1[0]; v11 = r1[1]; v12 = r1[2];
v20 = r2[0]; v21 = r2[1]; v22 = r2[2];
return (v00*v11*v22
+v01*v12*v20
+v02*v10*v21
-v02*v11*v20
-v01*v10*v22
-v00*v12*v21);
}
void MatrixAdj2x2 (float A[][], float &C[][]) { // Adjugate=Adjunkte 1x1, 2x2, 3x3
float r0[], r1[], r2[], c0[], c1[], c2[], t0, t1, t2;
ArrayIndex(r0, A, 0);
ArrayIndex(r1, A, 1);
ArrayIndex(t0, r1, 1);
ArrayIndex(t1, r0, 1);
asm { mul t1, t1, -1 }
ArrayBuild(c0, t0, t1);
ArrayIndex(t0, r1, 0);
asm { mul t0, t0, -1 }
ArrayIndex(t1, r0, 0);
ArrayBuild(c1, t0, t1);
ArrayBuild(C, c0, c1);
}
void MatrixAdj3x3 (float A[][], float &C[][]) { // Adjugate=Adjunkte 1x1, 2x2, 3x3
float r0[], r1[], r2[], c0[], c1[], c2[], t0, t1, t2;
float v00, v01, v02, v10, v11, v12, v20, v21, v22;
r0 = A[0]; r1 = A[1]; r2 = A[2];
v00 = r0[0]; v01 = r0[1]; v02 = r0[2];
v10 = r1[0]; v11 = r1[1]; v12 = r1[2];
v20 = r2[0]; v21 = r2[1]; v22 = r2[2];
t0 = v11*v22-v12*v21;
t1 = v02*v21-v01*v22;
t2 = v01*v12-v02*v11;
ArrayBuild(c0, t0, t1, t2);
t0 = v12*v20-v10*v22;
t1 = v00*v22-v02*v20;
t2 = v02*v10-v00*v12;
ArrayBuild(c1, t0, t1, t2);
t0 = v10*v21-v11*v20;
t1 = v01*v20-v00*v21;
t2 = v00*v11-v01*v10;
ArrayBuild(c2, t0, t1, t2);
ArrayBuild(C, c0, c1, c2);
}
bool MatrixInverse2x2(float A[][], float &C[][]) {
float det;
det=MatrixDet2x2(A);
if (det==0) return 0;
else {
MatrixAdj2x2(A, C);
C/=det;
return 1;
}
}
bool MatrixInverse3x3(float A[][], float &C[][]) {
float det;
det=MatrixDet3x3(A);
if (det==0) return 0;
else {
MatrixAdj3x3(A, C);
C/=det;
return 1;
}
}
void MatrixLinear(float A[][], float &C[], int R, int S) {
int i=0, s, r;
int dim=R*S;
ArrayInit(C, 0, dim);
for (s=0; s<S; ++s) {
for (r=0; r<R; ++r) {
C[i++]=A[r][s];
}
}
}
void SubMatrix(float A[][], float &C[][], int RS, int r, int s) {
int i=0, j=0, x=0, y=0, dim_1;
float tmp[];
dim_1=RS-1;
ArrayInit2D(C, tmp, 0, dim_1, dim_1);
for (y=0; y<RS; ++y) {
for (x=0; x<RS; ++x) {
if ((x!=r)&&(y!=s)) {
C[i][j]=A[x][y];
++i;
if (i>=dim_1){
i=0; ++j;
}
}
}
}
}
float MatrixMinor(float A[][], int RS, int r, int s) {
float C[][];
SubMatrix(A, C, RS, r, s);
if(RS==3) {
return MatrixDet2x2(C);
}
else
if(RS==4) {
return MatrixDet3x3(C);
}
}
float MatrixCofactor(float A[][], int RS, int r, int s) {
float x;
return pow(-1, r+s)* MatrixMinor(A, RS, r, s);;
}
float MatrixDet4x4(float A[][]) {
float det=0, x=0;
int i, j=0, n0, max0=0, imax0, jmax0=0;
for (j=0; j<4; ++j) { // find column with most 0's: jmax0
n0=0;
for (i=0; i<4; ++i) {
if (A[i][j]==0) ++n0;
}
if (max0<n0) {max0=n0; jmax0=j;}
}
for (i=0; i<4; ++i) { // find row with most 0's: imax0
n0=0;
for (j=0; j<4; ++j) {
if (A[i][j]==0) ++n0;
}
if (max0<n0) {max0=n0; imax0=i;}
}
if (jmax0>imax0) {
for (i=0; i<4; ++i) { // develop from jmax0
if ( A[i][jmax0]!=0) { // 0*?=0
x=A[i][jmax0]*MatrixCofactor(A, 4, i, jmax0);
det+=x;
}
}
//TextOut(72,56,"j"); NumOut(80,56,jmax0); //debug
return det;
}
else {
for (j=0; j<4; ++j) { // develop from imax0
if ( A[imax0][j]!=0) { // 0*?=0
x=A[imax0][j]*MatrixCofactor(A, 4, imax0, j);
det+=x;
}
}
//TextOut(72,56,"i"); NumOut(80,56,imax0); //debug
return det;
}
}
// speed test
long time0, runtime;
task main() {
float A[][], B[][], C[][], x;
float O[][], T[][], L[][], tmp[];;
int loop;
time0=CurrentTick();
for (loop=0; loop<100; ++loop)
{
ArrayInit2D(A, tmp, 0, 2,2);
ArrayInit2D(B, tmp, 0, 2,2);
ArrayInit2D(C, tmp, 0, 2,2);
ArrayInit2D(O, tmp, 0, 4,4);
NumOut(0,56,loop);
O[0][0]=0; O[0][1]=0; O[0][2]=8; O[0][3]=22;
O[1][0]=1; O[1][1]=5; O[1][2]=9; O[1][3]=13;
O[2][0]=2; O[2][1]=6; O[2][2]=10; O[2][3]=14;
O[3][0]=3; O[3][1]=7; O[3][2]=22; O[3][3]=15;
x=MatrixDet4x4(O);
SubMatrix(O,T,4, 2,1); // expanded to A[i][j]
x=MatrixDet3x3(T);
SubMatrix(T,C,3, 0,1);
x=MatrixDet2x2(C);
x=MatrixMinor(T, 3, 0,1);
x=MatrixCofactor(T, 3, 0,1);
A[0][0]=1; A[0][1]=3;
A[1][0]=2; A[1][1]=4;
x=10;
B=A; B*=x;
C=A; C+=B;
MatrixAdj2x2(A,C);
MatrixInverse2x2(A,C);
MatrixInverse2x2(A,B);
MatrixMatrixMult(A,B,C,2,2,2);
A[0][0]=1; A[0][1]=1;
A[1][0]=1; A[1][1]=0;
MatrixRotate2DGeo(A,B,30);
MatrixRotate2DGeo(A,C,90);
}
runtime=CurrentTick()-time0;
TextOut(0,40,"run time");
NumOut (0,32, runtime); TextOut(72,32,"msec");
while(1);
}
// old release 2010702:
// level 0: 9784
// level 1: 8462
// level 2: 7385
// level 3: 7385
// level 4: 7383
// new release 20110720:
// level 0: 9715
// level 1: 8470
// level 2: file error -1
// level 3: file error -1
// level 4: file error -1
the same for new fw, removed code
Code: Select all
#define ArrayIndex(_out, _A, _i) asm { index _out, _A, _i }
#define ArrayReplace(_A, _i, _new) asm { replace _A, _A, _i, _new }
#define BranchTest(_cmp, _lbl, _v1) asm{ brtst _cmp, _lbl, _v1 }
#define BranchComp(_cmp, _lbl, _v1, _v2) asm{ brcmp _cmp, _lbl, _v1, _v2 }