//////////////////////////////////////////////////////////////////////////////
//
//       Title     : exercise3.cpp
//       Project   : OSCAR tutorial
//       Created   : May 30, 1998
//       Author    : Mitch Pryor
//       Platforms : PC
//	 Copyright : Copyright© The University of Texas at Austin, 2002.
//                   All rights reserved.
//                 
//         This software and documentation constitute an unpublished work
//         and contain valuable trade secrets and proprietary information
//         belonging to University.  None of the foregoing material may be
//         copied  or duplicated or disclosed without the express, written
//         permission of University.  UNIVERSITY EXPRESSLY DISCLAIMS ANY
//         AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION,
//         INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
//         PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY
//         THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE.
//         NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF
//         THE SOFTWARE OR DOCUMENTATION.  Under no circumstances shall
//         University be liable for incidental, special, indirect, direct or
//         consequential damages or loss of profits, interruption of business,
//         or related expenses which may arise from use of software or documentation,
//         including but not limited to those resulting from defects in software
//         and/or documentation, or loss or inaccuracy of data of any kind.
//
//       Purpose   : Solution to Exercise 3 of the OSCAR Guide
//
//////////////////////////////////////////////////////////////////////////////
#include <iostream.h>

#include "Math/Xform.h"
#include "Math/HandPose.h"

int main(void)
{

	//Create and populate a Rotation Matrices
  RRRot3by3 rotA;
	rotA.at(0,0) =  cos(30*DEGTORAD);
  rotA.at(0,1) = -sin(30*DEGTORAD);
  rotA.at(1,0) =  sin(30*DEGTORAD);
  rotA.at(1,1) =  cos(30*DEGTORAD);
  RRRot3by3 rotB(rotA);
	
	//Create and populate translation vectors
	RRVector3 vA(10, 5, 2);
	RRVector3 vB(4, -3, 5);

	//Poplulate a tranform matrix from a rotation matrix and vector
	RRXform xA;
	xA.SetRotationMatrix(rotA);
	xA.SetTranslationVector(vA);
	
	RRXform xB;
	xB.SetRotationMatrix(rotB);
	xB.SetTranslationVector(vB);
    
	cout << xA << endl;

	RRHandPose handXYZ(RRHandPose::FIXED_XYZ);

	xA.ToHandPose(handXYZ);

	RRHandPose handZYZ(RRHandPose::EULER_ZYZ);
	xA.ToHandPose(handZYZ);
	
	cout << handXYZ << "\n" << handZYZ << endl;

	cout << (xA*xB).ToHandPose() << endl;

  
 	return 0;
}