Newsgroups: comp.robotics
Path: cantaloupe.srv.cs.cmu.edu!das-news2.harvard.edu!news2.near.net!satisfied.elf.com!news.mathworks.com!uunet!timbuk.cray.com!driftwood.cray.com!kilian
From: kilian@cray.com (Alan Kilian)
Subject: Re: Need: coordinate-transfomation
Message-ID: <1995Jan23.104342.24487@driftwood.cray.com>
Lines: 224
Nntp-Posting-Host: poplar021
Organization: Cray Research, Inc.
Date: 23 Jan 95 10:43:41 CST


I think that if you invert the matrix, you will get the rotation, and location
of the transmitter with respect to the receiver.

This code uses a 4x4 matrix where the upper left 3x3 matrix is the orientation
matrix, and the translation is along the bottom of the matrix.

Oh, that's hard to describe.

Translation goes in    

    _Ztmp[3][0] = x;
    _Ztmp[3][1] = y;
    _Ztmp[3][2] = z;
    _Ztmp[3][3] = 1.0;

So, fill in a 4x4 matrix with your 3x3 orientation and 1x3 translation, and 
invert it. 

You'll have to play with this code to get it to compile because I just ripped
it out of a graphics library I wrote.

Send me Email once you have tried it out.

                  -Alan Kilian

static char USMID[] = "@(#)Magic/lib/milt/MatInvert.c	11.0	01/10/95 11:02:15";
/*
 *	(C) COPYRIGHT CRAY RESEARCH, INC.
 *	UNPUBLISHED PROPRIETARY INFORMATION.
 *	ALL RIGHTS RESERVED.
 */

/*
 * 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 <math.h>

typedef float Zfloat;

typedef struct {
    Zfloat   matrix[4][4];
} ZMatrix;

ZInvertMatrix( in, out ) ZMatrix *in, *out;
{
    int i, j;
    float 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!\n");
        exit(1);
    }

    /* scale the adjoint matrix to get the inverse */

    for (i=0; i<4; i++)
        for(j=0; j<4; j++)
	    out->matrix[i][j] = out->matrix[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 ) ZMatrix *in; ZMatrix *out;
{
    float a1, a2, a3, a4, b1, b2, b3, b4;
    float c1, c2, c3, c4, d1, d2, d3, d4;
    float det3x3();

    /* assign to individual variable names to aid  */
    /* selecting correct values  */

	a1 = in->matrix[0][0]; b1 = in->matrix[0][1]; 
	c1 = in->matrix[0][2]; d1 = in->matrix[0][3];

	a2 = in->matrix[1][0]; b2 = in->matrix[1][1]; 
	c2 = in->matrix[1][2]; d2 = in->matrix[1][3];

	a3 = in->matrix[2][0]; b3 = in->matrix[2][1];
	c3 = in->matrix[2][2]; d3 = in->matrix[2][3];

	a4 = in->matrix[3][0]; b4 = in->matrix[3][1]; 
	c4 = in->matrix[3][2]; d4 = in->matrix[3][3];


    /* row column labeling reversed since we transpose rows & columns */

    out->matrix[0][0]  =   det3x3( b2, b3, b4, c2, c3, c4, d2, d3, d4);
    out->matrix[1][0]  = - det3x3( a2, a3, a4, c2, c3, c4, d2, d3, d4);
    out->matrix[2][0]  =   det3x3( a2, a3, a4, b2, b3, b4, d2, d3, d4);
    out->matrix[3][0]  = - det3x3( a2, a3, a4, b2, b3, b4, c2, c3, c4);
        
    out->matrix[0][1]  = - det3x3( b1, b3, b4, c1, c3, c4, d1, d3, d4);
    out->matrix[1][1]  =   det3x3( a1, a3, a4, c1, c3, c4, d1, d3, d4);
    out->matrix[2][1]  = - det3x3( a1, a3, a4, b1, b3, b4, d1, d3, d4);
    out->matrix[3][1]  =   det3x3( a1, a3, a4, b1, b3, b4, c1, c3, c4);
        
    out->matrix[0][2]  =   det3x3( b1, b2, b4, c1, c2, c4, d1, d2, d4);
    out->matrix[1][2]  = - det3x3( a1, a2, a4, c1, c2, c4, d1, d2, d4);
    out->matrix[2][2]  =   det3x3( a1, a2, a4, b1, b2, b4, d1, d2, d4);
    out->matrix[3][2]  = - det3x3( a1, a2, a4, b1, b2, b4, c1, c2, c4);
        
    out->matrix[0][3]  = - det3x3( b1, b2, b3, c1, c2, c3, d1, d2, d3);
    out->matrix[1][3]  =   det3x3( a1, a2, a3, c1, c2, c3, d1, d2, d3);
    out->matrix[2][3]  = - det3x3( a1, a2, a3, b1, b2, b3, d1, d2, d3);
    out->matrix[3][3]  =   det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3);
}
/*
 * float = det4x4( matrix )
 * 
 * calculate the determinant of a 4x4 matrix.
 */
float det4x4( m ) ZMatrix *m;
{
    float ans;
    float a1, a2, a3, a4, b1, b2, b3, b4, c1, c2, c3, c4, d1, d2, d3, 			d4;
    float det3x3();

    /* assign to individual variable names to aid selecting */
	/*  correct elements */

	a1 = m->matrix[0][0]; b1 = m->matrix[0][1]; 
	c1 = m->matrix[0][2]; d1 = m->matrix[0][3];

	a2 = m->matrix[1][0]; b2 = m->matrix[1][1]; 
	c2 = m->matrix[1][2]; d2 = m->matrix[1][3];

	a3 = m->matrix[2][0]; b3 = m->matrix[2][1]; 
	c3 = m->matrix[2][2]; d3 = m->matrix[2][3];

	a4 = m->matrix[3][0]; b4 = m->matrix[3][1]; 
	c4 = m->matrix[3][2]; d4 = m->matrix[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;
}



/*
 * float = 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 |
 */

float det3x3( a1, a2, a3, b1, b2, b3, c1, c2, c3 )
float a1, a2, a3, b1, b2, b3, c1, c2, c3;
{
    float ans;
    float det2x2();

    ans = a1 * det2x2( b2, b3, c2, c3 )
        - b1 * det2x2( a2, a3, c2, c3 )
        + c1 * det2x2( a2, a3, b2, b3 );
    return ans;
}

/*
 * float = det2x2( float a, float b, float c, float d )
 * 
 * calculate the determinant of a 2x2 matrix.
 */

float det2x2( a, b, c, d)
float a, b, c, d; 
{
    float ans;
    ans = a * d - b * c;
    return ans;
}
-- 
 -Alan Kilian    kilian@cray.com 612.683.5499 (Work)
  "Let us begin by approximating a Cow as a hollow sphere completely filled
   with milk." -A Physicist giving a lecture to the Dairy association.
