This BASIC code is based on the C/C++ code below. However, it does not appear to produce correct results. Can anyone point out what I am doing wrong?:
Const SMALL_NUMBER =0.00000001
Function InvertMatrix(in,out)
det#=det4x4(in)
If Abs(det)<SMALL_NUMBER
Return
EndIf
AdjointMatrix(in,out)
For i=0 To 15
PokeFloat out,i*4,PeekFloat(out,i*4)/det
Next
Return True
End Function
Function AdjointMatrix(in,out)
a1#=PeekFloat(in,0*4)
b1#=PeekFloat(in,1*4)
c1#=PeekFloat(in,2*4)
d1#=PeekFloat(in,3*4)
a2#=PeekFloat(in,4*4)
b2#=PeekFloat(in,5*4)
c2#=PeekFloat(in,6*4)
d2#=PeekFloat(in,7*4)
a3#=PeekFloat(in,8*4)
b3#=PeekFloat(in,9*4)
c3#=PeekFloat(in,10*4)
d3#=PeekFloat(in,11*4)
a4#=PeekFloat(in,12*4)
b4#=PeekFloat(in,13*4)
c4#=PeekFloat(in,14*4)
d4#=PeekFloat(in,15*4)
PokeFloat out,0*4,det3x3(b2,b3,b4,c2,c3,c4,d2,d3,d4)
PokeFloat out,4*4,-det3x3(a2,a3,a4,c2,c3,c4,d2,d3,d4)
PokeFloat out,8*4,det3x3(a2,a3,a4,b2,b3,b4,d2,d3,d4)
PokeFloat out,12*4,-det3x3(a2,a3,a4,b2,b3,b4,c2,c3,c4)
PokeFloat out,1*4,-det3x3(b1,b3,b4,c1,c3,c4,d1,d3,d4)
PokeFloat out,5*4,det3x3(a1,a3,a4,c1,c3,c4,d1,d3,d4)
PokeFloat out,9*4,-det3x3(a1,a3,a4,b1,b3,b4,d1,d3,d4)
PokeFloat out,13*4,det3x3(a1,a3,a4,b1,b3,b4,c1,c3,c4)
PokeFloat out,2*4,det3x3(b1,b2,b4,c1,c2,c4,d1,d2,d4)
PokeFloat out,6*4,-det3x3(a1,a2,a4,c1,c2,c4,d1,d2,d4)
PokeFloat out,10*4,det3x3(a1,a2,a4,b1,b2,b4,d1,d2,d4)
PokeFloat out,14*4,-det3x3(a1,a2,a4,b1,b2,b4,c1,c2,c4)
PokeFloat out,3*4,-det3x3(b1,b2,b3,c1,c2,c3,d1,d2,d3)
PokeFloat out,7*4,det3x3(a1,a2,a3,c1,c2,c3,d1,d2,d3)
PokeFloat out,11*4,-det3x3(a1,a2,a3,b1,b2,b3,d1,d2,d3)
PokeFloat out,15*4,det3x3(a1,a2,a3,b1,b2,b3,c1,c2,c3)
End Function
Function det4x4#(in)
a1#=PeekFloat(in,0*4)
b1#=PeekFloat(in,1*4)
c1#=PeekFloat(in,2*4)
d1#=PeekFloat(in,3*4)
a2#=PeekFloat(in,4*4)
b2#=PeekFloat(in,5*4)
c2#=PeekFloat(in,6*4)
d2#=PeekFloat(in,7*4)
a3#=PeekFloat(in,8*4)
b3#=PeekFloat(in,9*4)
c3#=PeekFloat(in,10*4)
d3#=PeekFloat(in,11*4)
a4#=PeekFloat(in,12*4)
b4#=PeekFloat(in,13*4)
c4#=PeekFloat(in,14*4)
d4#=PeekFloat(in,15*4)
Return a1*det3x3(b2,b3,b4,c2,c3,c4,d2,d3,d4)-b1*det3x3(a2,a3,a4,c2,c3,c4,d2,d3,d4)+c1*det3x3(a2,a3,a4,b2,b3,b4,d2,d3,d4)-d1*det3x3(a2,a3,a4,b2,b3,b4,c2,c3,c4)
End Function
Function det3x3#(a1#,a2#,a3#,b1#,b2#,b3#,c1#,c2#,c3#)
Return a1*det2x2(b2,b3,c2,c3)-b1*det2x2(a2,a3,c2,c3)+c1*det2x2(a2,a3,b2,b3)
End Function
Function det2x2#(a#,b#,c#,d#)
Return a*d-b*c
End Function
/*
Matrix Inversion
by Richard Carling
from "Graphics Gems", Academic Press, 1990
*/
#define SMALL_NUMBER 1.e-8
/*
* inverse( original_matrix, inverse_matrix )
*
* calculate the inverse of a 4x4 matrix
*
* -1
* A = ___1__ adjoint A
* det A
*/
#include "GraphicsGems.h"
#include <math.h>
inverse( in, out ) Matrix4 *in, *out;
{
int i, j;
double det, det4x4();
/* calculate the adjoint matrix */
adjoint( in, out );
/* calculate the 4x4 determinant
* if the determinant is zero,
* then the inverse matrix is not unique.
*/
det = det4x4( in );
if ( fabs( det ) < SMALL_NUMBER) {
printf("Non-singular matrix, no inverse!
");
exit(1);
}
/* scale the adjoint matrix to get the inverse */
for (i=0; i<4; i++)
for(j=0; j<4; j++)
out->element[i][j] = out->element[i][j] / det;
}
/*
* adjoint( original_matrix, inverse_matrix )
*
* calculate the adjoint of a 4x4 matrix
*
* Let a denote the minor determinant of matrix A obtained by
* ij
*
* deleting the ith row and jth column from A.
*
* i+j
* Let b = (-1) a
* ij ji
*
* The matrix B = (b ) is the adjoint of A
* ij
*/
adjoint( in, out ) Matrix4 *in; Matrix4 *out;
{
double a1, a2, a3, a4, b1, b2, b3, b4;
double c1, c2, c3, c4, d1, d2, d3, d4;
double det3x3();
/* assign to individual variable names to aid */
/* selecting correct values */
a1 = in->element[0][0]; b1 = in->element[0][1];
c1 = in->element[0][2]; d1 = in->element[0][3];
a2 = in->element[1][0]; b2 = in->element[1][1];
c2 = in->element[1][2]; d2 = in->element[1][3];
a3 = in->element[2][0]; b3 = in->element[2][1];
c3 = in->element[2][2]; d3 = in->element[2][3];
a4 = in->element[3][0]; b4 = in->element[3][1];
c4 = in->element[3][2]; d4 = in->element[3][3];
/* row column labeling reversed since we transpose rows & columns */
out->element[0][0] = det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
out->element[1][0] = - det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
out->element[2][0] = det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
out->element[3][0] = - det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
out->element[0][1] = - det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
out->element[1][1] = det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
out->element[2][1] = - det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
out->element[3][1] = det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);
out->element[0][2] = det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
out->element[1][2] = - det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
out->element[2][2] = det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
out->element[3][2] = - det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);
out->element[0][3] = - det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
out->element[1][3] = det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
out->element[2][3] = - det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
out->element[3][3] = det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
}
/*
* double = det4x4( matrix )
*
* calculate the determinant of a 4x4 matrix.
*/
double det4x4( m ) Matrix4 *m;
{
double ans;
double a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, d4;
double det3x3();
/* assign to individual variable names to aid selecting */
/* correct elements */
a1 = m->element[0][0]; b1 = m->element[0][1];
c1 = m->element[0][2]; d1 = m->element[0][3];
a2 = m->element[1][0]; b2 = m->element[1][1];
c2 = m->element[1][2]; d2 = m->element[1][3];
a3 = m->element[2][0]; b3 = m->element[2][1];
c3 = m->element[2][2]; d3 = m->element[2][3];
a4 = m->element[3][0]; b4 = m->element[3][1];
c4 = m->element[3][2]; d4 = m->element[3][3];
ans = a1 * det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4)
- b1 * det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4)
+ c1 * det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4)
- d1 * det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
return ans;
}
/*
* double = det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3 )
*
* calculate the determinant of a 3x3 matrix
* in the form
*
* | a1, b1, c1 |
* | a2, b2, c2 |
* | a3, b3, c3 |
*/
double det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3 )
double a1, a2, a3, b1, b2, b3, c1, c2, c3;
{
double ans;
double det2x2();
ans = a1 * det2x2( b2, b3, c2, c3 )
- b1 * det2x2( a2, a3, c2, c3 )
+ c1 * det2x2( a2, a3, b2, b3 );
return ans;
}
/*
* double = det2x2( double a, double b, double c, double d )
*
* calculate the determinant of a 2x2 matrix.
*/
double det2x2( a, b, c, d)
double a, b, c, d;
{
double ans;
ans = a * d - b * c;
return ans;
}