matrix.cpp

C arm 3D traslate 模擬C手臂在三度空間移動

// Matrix.cpp : implementation file
//

#include "stdafx.h"
#include <math.h>
#include "Matrix.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

/////////////////////////////////////////////////////////////////////////////
// CMatrix

IMPLEMENT_SERIAL(CMatrix, CObject, 0)

//default identity matrix
CMatrix::CMatrix()
{
  for(int i = 0; i < 3; i++ )
  {
    for(int j = 0; j < 3; j++ )
    {
      if( i == j )
        m_elem[i][j] = 1.0;
      else
        m_elem[i][j] = 0.0;
    }
  }
}

//set the diagonal entities equal to d.
CMatrix::CMatrix( double d)
{
  for(int i = 0; i < 3; i++ )
  {
    for(int j = 0; j < 3; j++ )
    {
      if( i == j )
        m_elem[i][j] = d;
      else
        m_elem[i][j] = 0.0;
    }
  }
}

//from column vectors
CMatrix::CMatrix( CVector& v1, CVector& v2, CVector& v3)
{
  m_elem[0][0] =  v1.X(); m_elem[1][0] =  v1.Y(); m_elem[2][0] =  v1.Z();
  m_elem[0][1] =  v2.X(); m_elem[1][1] =  v2.Y(); m_elem[2][1] =  v2.Z();
  m_elem[0][2] =  v3.X(); m_elem[1][2] =  v3.Y(); m_elem[2][2] =  v3.Z();
}

//3x3 elements
CMatrix::CMatrix( double elem[3][3] )
{
  for(int i = 0; i < 3; i++ )
    for(int j = 0; j < 3; j++ )
      m_elem[i][j] = elem[i][j];
}

CMatrix::~CMatrix()
{
}

//copy constructor
CMatrix::CMatrix( const CMatrix& m )
{ 
  DoCopy(m);
}

//assignment operator, cleanup & copy
CMatrix& CMatrix::operator= ( const CMatrix& m )
{
  //avoid self-assignment
  if( this == &m ) return *this;
  
  DoCopy(m);
  
  return *this;
}

void CMatrix::DoCopy( const CMatrix& m )
{
  for(int i = 0; i < 3; i++ )
    for(int j = 0; j < 3; j++ )
      m_elem[i][j] = m.m_elem[i][j];
}

//serialization
#define THIS_SCHEMA  1
void CMatrix::Serialize(CArchive& ar) 
{
	if (ar.IsStoring())
	{	
    ar << THIS_SCHEMA;
    SerializeThisSchema( ar );
	}
	else
	{	
    int nSchema;
    ar >> nSchema;
    
    switch( nSchema )
    {
    case 1: SerializeThisSchema( ar ); break;
    default:
      AfxThrowArchiveException( CArchiveException::badSchema );
      break;
    }
	}
}
#undef THIS_SCHEMA

void CMatrix::SerializeThisSchema(CArchive& ar) 
{
	if (ar.IsStoring())
	{	// storing code
    for(int i=0; i<3; i++ )
      for(int j=0; j<3; j++ )
        ar << m_elem[i][j];
	}
	else
	{	// loading code
    for(int i=0; i<3; i++ )
      for(int j=0; j<3; j++ )
        ar >> m_elem[i][j];
	}
}


/////////////////////////////////////////////////////////////////////////////////
//access
double CMatrix::Element( int i, int j ) const
{
  ASSERT( i >=0 && i <=2 );
  ASSERT( j >=0 && j <=2 );
  return m_elem[i][j];
}

CVector CMatrix::Row( int i ) const
{
  ASSERT( i >=0 && i <=2 );
  return CVector( m_elem[i][0], m_elem[i][1], m_elem[i][2] );
}

CVector CMatrix::Col( int j ) const
{
  ASSERT( j >=0 && j <=2 );
  return CVector( m_elem[0][j], m_elem[1][j], m_elem[2][j] );
}


void CMatrix::SetElement( int i, int j, double elem )
{
  ASSERT( i >=0 && i <=2 );
  ASSERT( j >=0 && j <=2 );
  m_elem[i][j] = elem;
}

void CMatrix::SetRow( int i, CVector& v )
{
  ASSERT( i >=0 && i <=2 );
  m_elem[i][0] = v[0];
  m_elem[i][1] = v[1];
  m_elem[i][2] = v[2];
}

void CMatrix::SetCol( int j, CVector& v )
{
  ASSERT( j >=0 && j <=2 );
  m_elem[0][j] = v[0];
  m_elem[1][j] = v[1];
  m_elem[2][j] = v[2];
}

//multiplication M = M1 * M2
TS_DLL_KERNEL CMatrix operator*(  CMatrix& a,  CMatrix& b )
{
#define ROWCOL(i, j) \
    a.m_elem[i][0]*b.m_elem[0][j] + \
    a.m_elem[i][1]*b.m_elem[1][j] + \
    a.m_elem[i][2]*b.m_elem[2][j]

    return CMatrix( CVector(ROWCOL(0,0), ROWCOL(1,0), ROWCOL(2,0) ),
                    CVector(ROWCOL(0,1), ROWCOL(1,1), ROWCOL(2,1) ),
                    CVector(ROWCOL(0,2), ROWCOL(1,2), ROWCOL(2,2) ) );
#undef ROWCOL
}

//comparision, accuray M_EPS10
BOOL CMatrix::operator==(  CMatrix& m ) const
{
  for( int i = 0; i < 3; i++ )
  {
    for( int j = 0; j < 3; j++ )
    {
      if( fabs( m_elem[i][j] - m.m_elem[i][j] ) > M_EPS10 )
        return FALSE;
    }
  }
  return TRUE;
}

//rotate a vector by a matrix, V = M * V1
TS_DLL_KERNEL CVector operator*(  CMatrix& m,  CVector& v )
{
  return CVector( v.m_coord[0] * m.m_elem[0][0] +
                  v.m_coord[1] * m.m_elem[0][1] +
                  v.m_coord[2] * m.m_elem[0][2] ,
                  v.m_coord[0] * m.m_elem[1][0] +
                  v.m_coord[1] * m.m_elem[1][1] +
                  v.m_coord[2] * m.m_elem[1][2] ,
                  v.m_coord[0] * m.m_elem[2][0] +
                  v.m_coord[1] * m.m_elem[2][1] +
                  v.m_coord[2] * m.m_elem[2][2] );
}

//transpose of matrix.
CMatrix CMatrix::Transpose() const
{
  return CMatrix( CVector( m_elem[0][0], m_elem[0][1], m_elem[0][2] ),
                  CVector( m_elem[1][0], m_elem[1][1], m_elem[1][2] ),
                  CVector( m_elem[2][0], m_elem[2][1], m_elem[2][2] ));
}

//determinant of the matrix
double CMatrix::Determinant() const
{
  return
    m_elem[0][0]*(m_elem[1][1]*m_elem[2][2] - m_elem[1][2] * m_elem[2][1]) -
    m_elem[0][1]*(m_elem[1][0]*m_elem[2][2] - m_elem[1][2] * m_elem[2][0]) +
    m_elem[0][2]*(m_elem[1][0]*m_elem[2][1] - m_elem[1][1] * m_elem[2][0]) ;
}