328 lines
7.5 KiB
C
328 lines
7.5 KiB
C
/*
|
|
* Copyright (c) 1992,1993-1995 Argonaut Technologies Limited. All rights reserved.
|
|
*
|
|
* $Id: matrix23.c 1.2 1995/08/31 16:29:32 sam Exp $
|
|
* $Locker: $
|
|
*
|
|
* A very unoptimised set of transforms - these should each
|
|
* be re-done for the sparse multiplication.
|
|
*
|
|
* 3x4 Matrices
|
|
*
|
|
*/
|
|
|
|
#include "fw.h"
|
|
#include "shortcut.h"
|
|
|
|
static char rscid[] = "$Id: matrix23.c 1.2 1995/08/31 16:29:32 sam Exp $";
|
|
|
|
static br_matrix23 mattmp1,mattmp2;
|
|
|
|
/*
|
|
* A = B
|
|
*/
|
|
void BR_PUBLIC_ENTRY BrMatrix23Copy(br_matrix23 *A, br_matrix23 *B)
|
|
{
|
|
A(0,0) = B(0,0);
|
|
A(0,1) = B(0,1);
|
|
|
|
A(1,0) = B(1,0);
|
|
A(1,1) = B(1,1);
|
|
|
|
A(2,0) = B(2,0);
|
|
A(2,1) = B(2,1);
|
|
}
|
|
|
|
/*
|
|
* A = B*C
|
|
*/
|
|
void BR_PUBLIC_ENTRY BrMatrix23Mul(br_matrix23 *A, br_matrix23 *B, br_matrix23 *C)
|
|
{
|
|
A(0,0) = BR_MAC2(B(0,0),C(0,0), B(0,1),C(1,0));
|
|
A(0,1) = BR_MAC2(B(0,0),C(0,1), B(0,1),C(1,1));
|
|
|
|
A(1,0) = BR_MAC2(B(1,0),C(0,0), B(1,1),C(1,0));
|
|
A(1,1) = BR_MAC2(B(1,0),C(0,1), B(1,1),C(1,1));
|
|
|
|
A(2,0) = BR_MAC2(B(2,0),C(0,0), B(2,1),C(1,0)) + C(2,0);
|
|
A(2,1) = BR_MAC2(B(2,0),C(0,1), B(2,1),C(1,1)) + C(2,1);
|
|
|
|
}
|
|
|
|
void BR_PUBLIC_ENTRY BrMatrix23Identity(br_matrix23 *mat)
|
|
{
|
|
M(0,0) = S1; M(0,1) = S0;
|
|
M(1,0) = S0; M(1,1) = S1;
|
|
M(2,0) = S0; M(2,1) = S0;
|
|
}
|
|
|
|
/*
|
|
* Ú ¿
|
|
* ³ cosé siné 0 ³
|
|
* ³ ³
|
|
* ³ -siné cosé 0 ³
|
|
* R(é) = ³ ³
|
|
* ³ 0 0 1 ³
|
|
* À Ù
|
|
*/
|
|
void BR_PUBLIC_ENTRY BrMatrix23Rotate(br_matrix23 *mat, br_angle rz)
|
|
{
|
|
br_scalar s = BR_SIN(rz);
|
|
br_scalar c = BR_COS(rz);
|
|
|
|
M(0,0) = c; M(0,1) = s;
|
|
M(1,0) = -s; M(1,1) = c;
|
|
M(2,0) = S0; M(2,1) = S0;
|
|
}
|
|
|
|
/*
|
|
* Ú ¿
|
|
* ³ 1 0 0 ³
|
|
* ³ ³
|
|
* ³ 0 1 0 ³
|
|
* T(dx,dy) = ³ ³
|
|
* ³ dx dy 1 ³
|
|
* À Ù
|
|
*/
|
|
void BR_PUBLIC_ENTRY BrMatrix23Translate(br_matrix23 *mat, br_scalar dx, br_scalar dy)
|
|
{
|
|
M(0,0) = S1; M(0,1) = S0;
|
|
M(1,0) = S0; M(1,1) = S1;
|
|
M(2,0) = dx; M(2,1) = dy;
|
|
}
|
|
|
|
/*
|
|
* Ú ¿
|
|
* ³ sx 0 0 ³
|
|
* ³ ³
|
|
* ³ 0 sy 0 ³
|
|
* S(sx,sy) = ³ ³
|
|
* ³ 0 0 1 ³
|
|
* À Ù
|
|
*/
|
|
void BR_PUBLIC_ENTRY BrMatrix23Scale(br_matrix23 *mat, br_scalar sx, br_scalar sy)
|
|
{
|
|
M(0,0) = sx; M(0,1) = S0;
|
|
M(1,0) = S0; M(1,1) = sy;
|
|
M(2,0) = S0; M(2,1) = S0;
|
|
}
|
|
|
|
/*
|
|
* Ú ¿
|
|
* ³ 1 sy 0 ³
|
|
* ³ ³
|
|
* ³ 0 1 0 ³
|
|
* ShearX(sy) = ³ ³
|
|
* ³ 0 0 1 ³
|
|
* À Ù
|
|
*/
|
|
void BR_PUBLIC_ENTRY BrMatrix23ShearX(br_matrix23 *mat, br_scalar sy)
|
|
{
|
|
M(0,0) = S1; M(0,1) = sy;
|
|
M(1,0) = S0; M(1,1) = S1;
|
|
M(2,0) = S0; M(2,1) = S0;
|
|
}
|
|
|
|
/*
|
|
* Ú ¿
|
|
* ³ 1 0 0 ³
|
|
* ³ ³
|
|
* ³ sx 1 0 ³
|
|
* ShearY(sx) = ³ ³
|
|
* ³ 0 0 1 ³
|
|
* À Ù
|
|
*/
|
|
void BR_PUBLIC_ENTRY BrMatrix23ShearY(br_matrix23 *mat, br_scalar sx)
|
|
{
|
|
M(0,0) = S1; M(0,1) = S0;
|
|
M(1,0) = sx; M(1,1) = S1;
|
|
M(2,0) = S0; M(2,1) = S0;
|
|
}
|
|
|
|
/*
|
|
* Computes the inverse of a 2D affine matrix;
|
|
*/
|
|
|
|
#define PRECISION_LIMIT BR_SCALAR(1e-15)
|
|
|
|
br_scalar BR_PUBLIC_ENTRY BrMatrix23Inverse(br_matrix23 *B, br_matrix23 *A)
|
|
{
|
|
br_scalar det,idet,pos = 0,neg = 0;
|
|
|
|
det = BR_MUL(A(0,0),A(1,1));
|
|
if (det>=0)
|
|
pos += det;
|
|
else
|
|
neg += det;
|
|
|
|
det = BR_MUL(-A(1,0),A(0,1));
|
|
if (det>=0)
|
|
pos += det;
|
|
else
|
|
neg += det;
|
|
|
|
det = pos+neg;
|
|
|
|
if (BR_ABS(det)<=BR_SCALAR_EPSILON*2)
|
|
return S0;
|
|
|
|
if (BR_DIV(det,pos-neg)<PRECISION_LIMIT)
|
|
return S0;
|
|
|
|
idet = BR_RCP(det);
|
|
|
|
B(0,0) = BR_MUL(A(1,1),idet);
|
|
B(0,1) = -BR_MUL(A(0,1),idet);
|
|
B(1,0) = -BR_MUL(A(1,0),idet);
|
|
B(1,1) = BR_MUL(A(0,0),idet);
|
|
B(2,0) = BR_MUL(BR_MUL(A(1,0),A(2,1))-BR_MUL(A(1,1),A(2,0)),idet);
|
|
B(2,1) = BR_MUL(BR_MUL(A(0,1),A(2,0))-BR_MUL(A(0,0),A(2,1)),idet);
|
|
|
|
return det;
|
|
}
|
|
|
|
/*
|
|
* Invert a length preserving matrix
|
|
*/
|
|
|
|
void BR_PUBLIC_ENTRY BrMatrix23LPInverse(br_matrix23 *B, br_matrix23 *A)
|
|
{
|
|
B(0,0) = A(1,1);
|
|
B(0,1) = -A(0,1);
|
|
B(1,0) = -A(1,0);
|
|
B(1,1) = A(0,0);
|
|
B(2,0) = BR_MUL(A(1,0),A(2,1))-BR_MUL(A(1,1),A(2,0));
|
|
B(2,1) = BR_MUL(A(0,1),A(2,0))-BR_MUL(A(0,0),A(2,1));
|
|
}
|
|
|
|
void BR_PUBLIC_ENTRY BrMatrix23LPNormalise(br_matrix23 *A, br_matrix23 *B)
|
|
{
|
|
BrVector2Normalise((br_vector2 *)A->m[Y],(br_vector2 *)B->m[Y]);
|
|
A(0,0) = A(1,1);
|
|
A(0,1) = -A(1,0);
|
|
|
|
A(2,0) = B(2,0);
|
|
A(2,1) = B(2,1);
|
|
}
|
|
|
|
/*
|
|
* [a b ] = [ e f ] . M
|
|
*/
|
|
void BR_PUBLIC_ENTRY BrMatrix23ApplyP(br_vector2 *A, br_vector2 *B, br_matrix23 *C)
|
|
{
|
|
A->v[0] = BR_MAC2(B->v[0],C(0,0), B->v[1],C(1,0)) + C(2,0);
|
|
A->v[1] = BR_MAC2(B->v[0],C(0,1), B->v[1],C(1,1)) + C(2,1);
|
|
}
|
|
|
|
/*
|
|
* [a b c] = [ e f 0 ] . M
|
|
*/
|
|
void BR_PUBLIC_ENTRY BrMatrix23ApplyV(br_vector2 *A, br_vector2 *B, br_matrix23 *C)
|
|
{
|
|
A->v[0] = BR_MAC2(B->v[0],C(0,0), B->v[1],C(1,0));
|
|
A->v[1] = BR_MAC2(B->v[0],C(0,1), B->v[1],C(1,1));
|
|
}
|
|
|
|
/*
|
|
* [a b] = [ e f] . transpose(M)
|
|
*/
|
|
void BR_PUBLIC_ENTRY BrMatrix23TApplyP(br_vector2 *A, br_vector2 *B, br_matrix23 *C)
|
|
{
|
|
A->v[0] = BR_MAC2(B->v[0],C(0,0), B->v[1],C(0,1));
|
|
A->v[1] = BR_MAC2(B->v[0],C(1,0), B->v[1],C(1,1));
|
|
}
|
|
|
|
/*
|
|
* [a b c] = [ e f 0 ] . transpose(M)
|
|
*/
|
|
void BR_PUBLIC_ENTRY BrMatrix23TApplyV(br_vector2 *A, br_vector2 *B, br_matrix23 *C)
|
|
{
|
|
A->v[0] = BR_MAC2(B->v[0],C(0,0), B->v[1],C(0,1));
|
|
A->v[1] = BR_MAC2(B->v[0],C(1,0), B->v[1],C(1,1));
|
|
}
|
|
|
|
/*
|
|
* Composite matrix operations -
|
|
* pre and post-multiply with an existing matrix
|
|
*/
|
|
|
|
void BR_PUBLIC_ENTRY BrMatrix23Pre(br_matrix23 *mat , br_matrix23 *A)
|
|
{
|
|
BrMatrix23Mul(&mattmp2,A,mat);
|
|
BrMatrix23Copy(mat,&mattmp2);
|
|
}
|
|
|
|
void BR_PUBLIC_ENTRY BrMatrix23Post(br_matrix23 *mat , br_matrix23 *A)
|
|
{
|
|
BrMatrix23Mul(&mattmp2,mat,A);
|
|
BrMatrix23Copy(mat,&mattmp2);
|
|
}
|
|
|
|
void BR_PUBLIC_ENTRY BrMatrix23PreRotate(br_matrix23 *mat, br_angle rz)
|
|
{
|
|
BrMatrix23Rotate(&mattmp1,rz);
|
|
BrMatrix23Mul(&mattmp2,&mattmp1,mat);
|
|
BrMatrix23Copy(mat,&mattmp2);
|
|
}
|
|
|
|
void BR_PUBLIC_ENTRY BrMatrix23PostRotate(br_matrix23 *mat, br_angle rz)
|
|
{
|
|
BrMatrix23Rotate(&mattmp1,rz);
|
|
BrMatrix23Mul(&mattmp2,mat,&mattmp1);
|
|
BrMatrix23Copy(mat,&mattmp2);
|
|
}
|
|
|
|
void BR_PUBLIC_ENTRY BrMatrix23PreTranslate(br_matrix23 *mat, br_scalar x, br_scalar y)
|
|
{
|
|
BrMatrix23Translate(&mattmp1,x,y);
|
|
BrMatrix23Mul(&mattmp2,&mattmp1,mat);
|
|
BrMatrix23Copy(mat,&mattmp2);
|
|
}
|
|
|
|
void BR_PUBLIC_ENTRY BrMatrix23PostTranslate(br_matrix23 *A, br_scalar x, br_scalar y)
|
|
{
|
|
A(2,0) += x;
|
|
A(2,1) += y;
|
|
}
|
|
|
|
void BR_PUBLIC_ENTRY BrMatrix23PreScale(br_matrix23 *mat, br_scalar sx, br_scalar sy)
|
|
{
|
|
BrMatrix23Scale(&mattmp1,sx,sy);
|
|
BrMatrix23Mul(&mattmp2,&mattmp1,mat);
|
|
BrMatrix23Copy(mat,&mattmp2);
|
|
}
|
|
|
|
void BR_PUBLIC_ENTRY BrMatrix23PostScale(br_matrix23 *mat, br_scalar sx, br_scalar sy)
|
|
{
|
|
BrMatrix23Scale(&mattmp1,sx,sy);
|
|
BrMatrix23Mul(&mattmp2,mat,&mattmp1);
|
|
BrMatrix23Copy(mat,&mattmp2);
|
|
}
|
|
|
|
void BR_PUBLIC_ENTRY BrMatrix23PreShearX(br_matrix23 *mat, br_scalar sy)
|
|
{
|
|
BrMatrix23ShearX(&mattmp1,sy);
|
|
BrMatrix23Mul(&mattmp2,&mattmp1,mat);
|
|
BrMatrix23Copy(mat,&mattmp2);
|
|
}
|
|
|
|
void BR_PUBLIC_ENTRY BrMatrix23PostShearX(br_matrix23 *mat, br_scalar sy)
|
|
{
|
|
BrMatrix23ShearX(&mattmp1,sy);
|
|
BrMatrix23Mul(&mattmp2,mat,&mattmp1);
|
|
BrMatrix23Copy(mat,&mattmp2);
|
|
}
|
|
|
|
void BR_PUBLIC_ENTRY BrMatrix23PreShearY(br_matrix23 *mat, br_scalar sx)
|
|
{
|
|
BrMatrix23ShearY(&mattmp1,sx);
|
|
BrMatrix23Mul(&mattmp2,&mattmp1,mat);
|
|
BrMatrix23Copy(mat,&mattmp2);
|
|
}
|
|
|
|
void BR_PUBLIC_ENTRY BrMatrix23PostShearY(br_matrix23 *mat, br_scalar sx)
|
|
{
|
|
BrMatrix23ShearY(&mattmp1,sx);
|
|
BrMatrix23Mul(&mattmp2,mat,&mattmp1);
|
|
BrMatrix23Copy(mat,&mattmp2);
|
|
}
|