/ray/src/lib/numerics/3d/omatrix.cc

Go to the documentation of this file.

/*
 * lib/numerics/3d/omatrix.cc
 * 
 * 3x4 object transform matrix. 
 * 
 * Copyright (c) 2004 by Wolfgang Wieser ] wwieser (a) gmx <*> de [ 
 * 
 * This file may be distributed and/or modified under the terms of the 
 * GNU General Public License version 2 as published by the Free Software 
 * Foundation. (See COPYING.GPL for details.)
 * 
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 * 
 */

#include "omatrix.h"

namespace NUM   // numerics
{


// This is a hack which makes sure that the required instances for dbl 
// and flt are emitted. 
void _OMatrixInstantiationDummy()
{
    {
        OMatrix<dbl> *a=NULL,b(OMatrix<dbl>::Mul,*a,*a);
        *a*=b; *a/=b;
        Vector3<dbl> *v=NULL;
        OMatrix<dbl> c(OMatrix<dbl>::Rotate,*v,0.1);
        OMatrix<dbl> d(OMatrix<dbl>::Invers,c);
    }
    {
        OMatrix<flt> *a=NULL,b(OMatrix<flt>::Mul,*a,*a);
        *a*=b; *a/=b;
        Vector3<flt> *v=NULL;
        OMatrix<flt> c(OMatrix<flt>::Rotate,*v,0.1);
        OMatrix<flt> d(OMatrix<flt>::Invers,c);
    }
}


template<typename T>OMatrix<T>::OMatrix(
    enum OMatrix<T>::_Mul,const OMatrix<T> &a,const OMatrix<T> &b)
{
    for(short int i=0; i<3; i++)
    {
        for(short int j=0; j<3; j++)
        {   m[i][j] = a.m[i][0]*b.m[0][j]
                    + a.m[i][1]*b.m[1][j]
                    + a.m[i][2]*b.m[2][j];  }
        m[i][3] = a.m[i][0]*b.m[0][3]
                + a.m[i][1]*b.m[1][3]
                + a.m[i][2]*b.m[2][3]
                + a.m[i][3];
    }
}


template<typename T>OMatrix<T> &OMatrix<T>::operator*=(const OMatrix<T> &b)
{
    T tmp[4];
    
    for(short int i=0; i<3; i++)
    {
        // Compute row [i] in result matrix. 
        tmp[0]=m[i][0];  tmp[1]=m[i][1];  tmp[2]=m[i][2];  tmp[3]=m[i][3];
        
        for(short int j=0; j<3; j++)
        {   m[i][j] = tmp[0]*b.m[0][j]
                    + tmp[1]*b.m[1][j]
                    + tmp[2]*b.m[2][j];  }
        
        m[i][3] = tmp[0]*b.m[0][3]
                + tmp[1]*b.m[1][3]
                + tmp[2]*b.m[2][3]
                + tmp[3];
    }
    
    return(*this);
}


template<typename T>OMatrix<T> &OMatrix<T>::operator/=(const OMatrix<T> &a)
{
    T tmp[3];
    
    for(short int j=0; j<3; j++)
    {
        // Compute column [j] in result matrix. 
        tmp[0]=m[0][j];  tmp[1]=m[1][j];  tmp[2]=m[2][j];
        
        for(short int i=0; i<3; i++)
        {   m[i][j] = a.m[i][0]*tmp[0]
                    + a.m[i][1]*tmp[1]
                    + a.m[i][2]*tmp[2];  }
    }
    
    // Compute column [3] (last one) in result matrix. 
    tmp[0]=m[0][3];  tmp[1]=m[1][3];  tmp[2]=m[2][3];
    for(short int j=0; j<3; j++)
    {
        m[j][3] = a.m[j][0]*tmp[0]
                + a.m[j][1]*tmp[1]
                + a.m[j][2]*tmp[2]
                + a.m[j][3];
    }
    
    return(*this);
}


template<typename T>OMatrix<T>::OMatrix(
    enum OMatrix<T>::_Rotate,const Vector3<T> &axis,T angle)
{
    T s,c;
    sincos(angle,&s,&c);
    T c1=1-c;
    
    
    T sq;
    sq=sqr(axis[0]);  m[0][0] = sq + c*(1-sq);
    sq=sqr(axis[1]);  m[1][1] = sq + c*(1-sq);
    sq=sqr(axis[2]);  m[2][2] = sq + c*(1-sq);
    
    T tmp = axis[0]*axis[1]*c1;
    T tmp2 = axis[2]*s;
    m[0][1] = tmp + tmp2;
    m[1][0] = tmp - tmp2;
    
    tmp = axis[0]*axis[2]*c1;
    tmp2 = axis[1]*s;
    m[0][2] = tmp - tmp2;
    m[2][0] = tmp + tmp2;
    
    tmp = axis[1]*axis[2]*c1;
    tmp2 = axis[0]*s;
    m[1][2] = tmp + tmp2;
    m[2][1] = tmp - tmp2;
}


template<typename T>OMatrix<T>::OMatrix(
    enum OMatrix<T>::_Invers,const OMatrix<T> &a)
{
    // This is turbo-inversion; self-derived formula for my strange 
    // 3x4 matrices. But fortunately, it turns out that these matrices 
    // CAN be inverted and that the inversion is quite much faster 
    // (in number of multiplications) than the 4x4 matrix inversion. 
    // This rocks!                                 (Wolfgang Wieser)
    
    T a0211 = a[0][2]*a[1][1];
    T a2201 = a[2][2]*a[0][1];
    T a1221 = a[1][2]*a[2][1];
    T a0022 = a[0][0]*a[2][2];
    T a2012 = a[2][0]*a[1][2];
    T a1002 = a[1][0]*a[0][2];
    // Calculate the determinant. fact is then 1/determinant. 
    T fact = a0022*a[1][1] - a0211*a[2][0]
           + a2012*a[0][1] - a2201*a[1][0]
           + a1002*a[2][1] - a1221*a[0][0];
    fact=1/fact;
    
    // First row of res: 
    m[0][0] = (a[1][1]*a[2][2] - a1221)*fact;
    m[0][1] = (a[2][1]*a[0][2] - a2201)*fact;
    m[0][2] = (a[0][1]*a[1][2] - a0211)*fact;
    
    // Second row of m: 
    m[1][0] = (a2012 - a[1][0]*a[2][2])*fact;
    m[1][1] = (a0022 - a[2][0]*a[0][2])*fact;
    m[1][2] = (a1002 - a[0][0]*a[1][2])*fact;
    
    // Third row of m: 
    m[2][0] = (a[1][0]*a[2][1] - a[2][0]*a[1][1])*fact;
    m[2][1] = (a[2][0]*a[0][1] - a[0][0]*a[2][1])*fact;
    m[2][2] = (a[0][0]*a[1][1] - a[1][0]*a[0][1])*fact;
    
    // Translation column of m: 
    // Note: This uses already computed values from the other part of the 
    //       result matrix. 
    m[0][3] = - m[0][0] * a[0][3] - m[0][1] * a[1][3] - m[0][2] * a[2][3];
    m[1][3] = - m[1][0] * a[0][3] - m[1][1] * a[1][3] - m[1][2] * a[2][3];
    m[2][3] = - m[2][0] * a[0][3] - m[2][1] * a[1][3] - m[2][2] * a[2][3];
}

}  // end of namespace NUM


#if 0
// Little test program. 

typedef NUM::OMatrix<dbl> OMatrix;

#include <stdio.h>  /* test program */
#include <stdlib.h>

static void Print(const OMatrix &m)
{
    for(int r=0; r<3; r++)
    {
        for(int c=0; c<4; c++)
        {
            char tmp[32];
            snprintf(tmp,32,"%g",m[r][c]);
            printf("%12s  ",tmp);
        }
        printf("\n");
    }
}

int main()
{
    OMatrix a,b;
    for(int i=0; i<20; i++)
    {
        // Yeah... this is a hack but I just wanna test the stuff!
        for(int i=0; i<12; i++)
        {
            a[0][i]=dbl(rand())/RAND_MAX*10;
            b[0][i]=dbl(rand())/RAND_MAX*10;
        }
        
        OMatrix p1=a*b;
        OMatrix p2=a;  p2*=b;
        OMatrix p3=b;  p3/=a;
        
        if(!NUM::equal(p1,p2,1e-10) || !NUM::equal(p1,p3,1e-10))
        {  printf("OOPS: NEQ\n");  }
    }
    
    for(int _i=0; _i<20; _i++)
    {
        // Yeah... this is a hack but I just wanna test the stuff!
        for(int i=0; i<12; i++)
        {  a[0][i]=dbl(rand())/RAND_MAX*10;  }
        
        OMatrix inv(OMatrix::Invers,a);
        
        b=inv*a-OMatrix(OMatrix::Ident);
        if(!b.IsNull(1e-12))
        {  printf("OOPS(INV)\n");  Print(b);  }
        
        b=a*inv-OMatrix(OMatrix::Ident);
        if(!b.IsNull(1e-12))
        {  printf("OOPS(INV)\n");  Print(b);  }
    }
    
    printf("DONE\n");
}
#endif

---