mattest.cpp

用C++写的矩阵和矢量运算库

  CWVector v4( v3 , 1 , 2 );

  v3.MapInto( m0 , 1 , 2 , 3 );
  v3.MapInto( v1 , 1 , 2 );

  CWVector v5;
  v5.Dimension( 3 );

  double d = v0[0];
  cout << "d = " << d << endl;

  CWVector v6( 3 );
  v6[0] = 2.0;
  v6[1] = 2.0;
  v6[2] = 2.0;
  cout << "v6 = " << v6 << endl;

  cout << "v0 + v6 = " << (v0 + v6) << endl;
  cout << "v0 - v6 = " << (v0 - v6) << endl;
  cout << "v0 * v6 = " << (v0 * v6) << endl;
  cout << "v0 / 3.0 = " << (v0 / 3.0) << endl;

  CWVector v7( 3 );

  v7 = v6;
  v7 += v6;
  v7 -= v6;
  v7 *= 3.0;
  v7 /= 3.0;

  v7.StoreAtRow( 0 , v6 );

  CWMatrix m2( 3 , 3 );
  CWVector v8 = m2*v0;

  //
  // Test Phase 2. Check the mathimatical validty of the vector template
  //

  // Some checks are going to be done with 2x2 matrices and other 3x3 matrices.
  v6[0] = 3.0;
  v6[1] = 4.0;
  v6[2] = 5.0;
  cout << "v6 = " << v6 << endl;

  cout << "v0 + v6 = " << (v0 + v6) << endl;
  cout << "v0 - v6 = " << (v0 - v6) << endl;
  cout << "v0 * v6 = " << (v0 * v6) << endl;
  cout << "v0 / 3.0 = " << (v0 / 3.0) << endl;
  cout << "norm as !v6 " << !v6 << endl;
  cout << "norm as norm(v6) " << norm(v6) << endl;
  cout << "unit vector v6.Unit() " << v6.Unit() << endl;

  v6.MakeUnit();
  cout << "v6.MakeUnit() " << v6 << endl;

  // Test matrix*vector.
  CWMatrix m3( 2 , 3 );
  m3.Fill(1.0);
  cout << "m3 * v6 = " << (m3 * v6) << endl;
	
  cout << endl << "End Vector test" << endl;

}

// Test Square Matrix

void test_smatrix( )
{

  cout << endl << "Start Square Matrix test" << endl;
  //
  // Test Phase 1. Check the mechcanics of template.
  //
  typedef CWTSquareMatrix< CWTSquareMatrix<> > CWMatrixMatrix;

  // allocate a 2x2 matrix
  // This only allocates the elements. not what is in the elements.
  CWMatrixMatrix mm0( 2 );
  CWMatrixMatrix mm1( 2 );
  // allocate each element
  CWSquareMatrix smatTmp( 2 );
  mm0[0][0] = smatTmp;
  mm0[0][1] = smatTmp;
  mm0[1][0] = smatTmp;
  mm0[1][1] = smatTmp;
  mm1[0][0] = smatTmp;
  mm1[0][1] = smatTmp;
  mm1[1][0] = smatTmp;
  mm1[1][1] = smatTmp;

  CWSquareMatrix mIdentity( 2 );
  mIdentity.MakeUnity();
  // fill it with a bunch of Identity matrices.
  mm1.Fill( mIdentity );
  mm0.Fill( mIdentity );
  cout << endl << "mm0 = " << mm0 << endl;
  cout << endl << "mm1 = " << mm1 << endl;

  CWMatrixMatrix mm3 = mm0 + mm1;
  cout << endl << "mm3 0,0 = " << mm3[0][0] << endl;
  cout << endl << "mm3 0,1 = " << mm3[1][0] << endl;
  cout << endl << "mm3 1,0 = " << mm3[0][1] << endl;
  cout << endl << "mm3 1,1 = " << mm3[1][1] << endl;
  // or perhaps
  cout << endl << "mm3 = " << mm3 << endl;
	
  // Test trace
  cout << endl << "Calculate the trace of a unity matrix." << endl;

  for (unsigned c = 1; c <= 5; c++)
    {
      CWSquareMatrix smtx(c);
      smtx.MakeUnity();
      cout << "smtx(" << smtx.GetCols() << ") = " << smtx
	   << " tr(smtx) = " << tr(smtx) << endl << endl;
    }

  //CWMatrixOfSquareMatrices mosm( 3 , 3 );

  // Test determinant
  cout << endl << "Calculate the determinant of a matrix." << endl;

  CWSquareMatrix smtx(3);

  smtx[0][0] = 2; smtx[0][1] =  3; smtx[0][2] = -1;
  smtx[1][0] = 3; smtx[1][1] =  5; smtx[1][2] =  2;
  smtx[2][0] = 1; smtx[2][1] = -2; smtx[2][2] = -3;

  cout << "mtx = " << smtx << " det(mtx) = " << det(smtx) << endl;

  //
  // Test Phase 2. Check the mathimatical validty of the vector template
  //
	
  cout << endl << "End Square Matrix test" << endl;

}

// Test space vector

void test_svector( )
{

  cout << endl << "Start Space Vector test" << endl;
  //
  // Test Phase 1. Check the mechcanics of template.
  //
  CWSpaceVector v0, v1;
  v0[0] = 1.0;
  v0[1] = 0.0;
  v0[2] = 0.0;
  cout << "v0 = " << v0 << endl;

  v1[0] = 0.0;
  v1[1] = 1.0;
  v1[2] = 0.0;
  cout << "v1 = " << v1 << endl;

  cout << "v0*v1 = " << v0*v1 << endl;
  cout << "v0%v1 = " << v0%v1 << endl;

  // without the namespace gcc complains.
  CwMtx::CWTSpaceVector<> v2;

  //
  // Test Phase 2. Check the mathimatical validty of the space vector template
  //

  cout << endl << "End Space Vector test" << endl;

}

// Test Quaternions

void test_quaternions( )
{
  // VC++ is more compliant with namespaces.  I can't seem to find the
  // exact pattern at which gcc begins to complain.  It seems that gcc
  // ignores template functions being in namespaces. That is ok as
  // templates functions are always specific to templates classes so
  // there is rarely namespace clashes. Gcc will complain about
  // namespace directives missing for template though.
  using namespace CwMtx;
  using namespace std;

  // just making sure the matrix namespace doesn't clash with something else.
  // it meant as a test against gcc's namespace handling.
  list<int> x;

  cout << endl << "Start Quaternion test" << endl;
  //
  // Test Phase 1. Check the mechcanics of template.
  //
  // 1-axis
  CWSpaceVector vAxis( 0.0 , 1.0 , 0.0 );
  // rotate 45.0 degrees about Y-axis.
  CWQuaternion q0( QtnFromAxisAngle( vAxis , 45.0/57.3 ) );
  cout << "q0 = " << q0 << endl;
  CWVector vAxisAngle = AxisAngleFromQtn( q0 );
  cout << "vAxisAngle = " << vAxisAngle << endl;
	
  // Test conjugate
  cout << endl << "Calculate the conjugate of a quaternion." << endl;

  CWQuaternion qtn(1, 2, 3, 4);

  cout << "qtn = " << qtn << endl
       << "conj(qtn) = " << conj(qtn) << endl
       << "qtn*conj(qtn) = " << qtn*conj(qtn) << endl;

  CWQuaternion q0Conj;
  q0Conj.StoreConjugate( q0 );
  cout << "q0Conj = " << q0Conj << endl;
  // or
  cout << "conjugate q0 = " << conj(q0) << endl;

  CWSpaceVector vUnit( 1.0 , 1.0 , 0.0 );
  // should give (0.707 , 0.707 , 0.0)
  cout << "Unit axis = " << vUnit.Unit() << endl;

  //
  // Test Phase 2. Check the mathimatical validty of the Quaternions template
  //
  // point to be rotated in the X,Z plane.
  CWQuaternion qPoint( 2.0 , 0.0 , 0.0 , 0.0 );
  // For a right-handed system this should rotate the point CCW to
  // (1.414 , 0 , -1.414)
  // q1 is now a rotated point.
  CWQuaternion q1 = q0 * qPoint * conj(q0);
  cout << "q1 = " << q1 << endl;

  vAxisAngle = AxisAngleFromQtn( q1 );
  cout << "vAxisAngle = " << vAxisAngle << endl;

  CWSpaceVector vNewAxis;
  vNewAxis.MapInto( vAxisAngle );
  cout << "vNewAxis = " << vNewAxis << endl;
  // or
  CWSpaceVector vNewAxis2;
  // map quaternion into a space vector
  vNewAxis2.MapInto(  q1 , 0 );
  cout << "vNewAxis2 = " << vNewAxis2 << endl;
  // or
  CWVector vNewAxis3;
  vNewAxis3.MapInto( vAxisAngle , 0 , 2 );
  cout << "vNewAxis3 = " << vNewAxis3 << endl;

  cout << "Norm of vNewAxis = " << vNewAxis.Norm() << endl;
  cout << "Unit norm of vNewAxis = " << vNewAxis.Unit() << endl;

  // Test quaternion division
  CWQuaternion qtn1(0.5*sqrt(2.0), 0.5*sqrt(2.0), 0, 0);
  CWQuaternion qtn2(0, 0, sqrt(2.0), sqrt(2.0));
  cout << endl << "Quaternion division" << endl << endl
       << "qtn1 = " << qtn1 << endl
       << "qtn2 = " << qtn2 << endl
       << "(qtn1*qtn2)/qtn2 = " << (qtn1*qtn2)/qtn2 << endl
       << "(qtn1*qtn2)/qtn1 = " << (qtn1*qtn2)/qtn1 << endl;

  // Test QtnFromSmat
  cout << endl << "Calculate quaternion from a transformation matrix." << endl;

  const double DEG2RAD = M_PI/180.0;
  for (double psi = 0; psi < 360; psi += 45)
    {
      for (double theta = 0; theta < 360; theta += 45)
	{
	  for (double phi = 0; phi < 360; phi += 45)
	    {
	      CWTSquareMatrix<double> smat =
		SmatFromEuler321Angles(psi*DEG2RAD,
				       theta*DEG2RAD,
				       phi*DEG2RAD);
	      CWTQuaternion<double> qtn = QtnFromSmat(smat);

	      int i = 0;
	      if (smat[1][1] > smat[0][0]) i = 1;
	      if (smat[2][2] > smat[i][i]) i = 2;

	      cout << "psi = " << psi
		   << ", theta = " << theta
		   << ", phi = " << phi << endl
		   << "trace = " << tr(smat) << ", case label: " << i << endl
		   << "psi = " << Euler321Angle3FromQtn(qtn)/DEG2RAD
		   << ", theta = " << Euler321Angle2FromQtn(qtn)/DEG2RAD
		   << ", phi = " << Euler321Angle1FromQtn(qtn)/DEG2RAD << endl
		   << "qtn = " << qtn << endl
		   << "qtnEuler = " << QtnFromEuler321Angles(psi*DEG2RAD,
							     theta*DEG2RAD,
							     phi*DEG2RAD)
		   << endl << endl;
	    }
	}
    }

  // Test quaternions constructed from exponential form.
  cout << endl
       << "Test quaternions constructed from exponential form." << endl;

  CWMatrix qeMat(3, 1);
  qeMat[0][0] = 1.0;
  qeMat[1][0] = 0.0;
  qeMat[2][0] = 0.0;

  CWSpaceVector qeSVec(-1.4, 3.2, 1.7);

  CWQuaternion qe0(1.0, qeMat, (45.0 / 180.0) * M_PI);
  CWQuaternion qe1(1.7, qeSVec, (-45.0 / 180.0) * M_PI);
  CWQuaternion qe2(-0.7, CWSpaceVector(0.4, 0.2, -0.7), (25.0 / 180.0) * M_PI);

  // should give
  // 0.7071, 0, 0, 0.7071 (real)
  cout << "qe0 = " << qe0 << endl;
  // 0.4332, -0.9902, -0.5261, 1.202 (real)
  cout << "qe1 = " << qe1 << endl;
  // -0.1425, -0.07123, 0.2493, -0.6344 (real)
  cout << "qe2 = " << qe2 << endl;

  cout << endl << "End Quaternion test" << endl;

}

// Test coordinate system template functions.

void test_coordsys( )
{

  // using gcc 2.95.2 the namespace can be left out. VC++ is more
  // strict and it must be present
  using namespace CwMtx;

  cout << endl << "Start Coordinate System test" << endl;
  //
  // Test Phase 1. Check the mechcanics of template.
  //
  CWSquareMatrix sm0( 3 );
  double d = Euler321Angle3FromSmat( sm0 );
  cout << "d = " << d << endl;

  //
  // Test Phase 2. Check the mathimatical validty of the coordinate template
  //

  // Test vectors in vectors
  typedef CWTSpaceVector< CWTSpaceVector<> > CWSpaceVectors;

  CWSpaceVectors vStandardBasisFrame;
  // x-axis or right
  vStandardBasisFrame[0] = CWSpaceVector( 1.0 , 0.0 , 0.0 );
  // y-axis or up
  vStandardBasisFrame[1] = CWSpaceVector( 0.0 , 1.0 , 0.0 );
  // z-axis or line of sight
  vStandardBasisFrame[2] = CWSpaceVector( 0.0 , 0.0 , 1.0 );
  // or
  CWSpaceVectors vObjectFrame(CWSpaceVector( 0.707 , 0.0 , 0.707 ),
			      CWSpaceVector( 0.0 , 1.0 , 0.0 ),
			      CWSpaceVector( 0.707 , 0.0 , 0.707 ));
	
  // Get the rotation matrix that represents a rotation from the
  // standard basis to the object's frame where a frame represents an
  // orientation.
  CWSquareMatrix rM( ChangeOfBasis( vObjectFrame , vStandardBasisFrame ) );
  cout << "rM = " << rM << endl;

  // convert matrix to quaternion and get conjugate. We want the
  // conjugate because we want to feed OpenGL a rotatation that
  // rotates the object from standard frame to the frame of the
  // object.  Typically this quaternion is converted to a AxisAngle
  // combo and passed to the glRotated method which just happens to
  // take an angle(degrees) plus an axis.
  CWQuaternion q = conj( QtnFromSmat( rM ) );

  // For example, if I were to call glRotate then I would do somthing
  // similar from "test_quaternions" method. Note: HERE I AM
  // INTERPRITING {q} TO REPRESENT AN ORIENTATION.
  CWVector vAxisAngle = AxisAngleFromQtn( q );
  // The following is real working code that I use in practice.
  // glPushMatrix(); glRotated( vAxisAngle[3]*57.3 , vAxisAngle[0],
  // vAxisAngle[1] , vAxisAngle[2] ); Draw object here glPopMatrix();

  // Or, we can apply that rotation to some other object, given that
  // the object's default local orientation/frame is the standard
  // basis.  Note: HERE I AM INTERPRITING {q} TO REPRESENT A ROTATION.
  CWSpaceVector vSomePointInAnObject( 0.7 , 1.0 , 1.2 );
  // map from [vector space] into [quaternion space].
  CWQuaternion qSomePointInAnObject( vSomePointInAnObject , 0.0 );

  // or perhaps we overite
  qSomePointInAnObject = q * qSomePointInAnObject * conj(q);
  // map quaternion back into vector space
  vSomePointInAnObject.MapInto( qSomePointInAnObject , 0 );

  //
  // Test Phase 3
  //

  // Can't be tested until the other sibling classes are converted to templates

  cout << endl << "End Coordinate System test" << endl;

}

int main(int argc, char *argv[])
{
  test_matrix();
  test_vector();
  test_smatrix();
  test_svector();
  test_quaternions();
  test_coordsys();
  test_struct_matrix();

  return 0;
}