Tutorial: demo_coords.cpp

Tutorial on how to perform 3D coordinate manipulation (rotation and translations of points, frames, etc.) thank to C++ objects and custom operators introduced in Chrono::Engine.
This example does not show any GUI (graphical user interface).

Let's start

First of all, include the needed headers.

 
#include "core/CHlog.h"
#include "core/CHtrasform.h"
#include "core/CHframe.h"
#include "core/CHframeMoving.h"
#include "physics/CHapidll.h"

Note that Chrono::Engine classes and functions belong to custom namespaces, in order to avoid names pollution when using other complex libraries. Therefore, remember to write...


using namespace chrono;

Now perform some test operations, in the main() function. Before beginning, print some message on the console and initialize global variables of the Chrono::Engine library:

		        
int main(int argc, char* argv[])
{
	GetLog() << "CHRONO demo about coordinate transformations: \n\n";

	ChGlobals* GLOBAL_Vars = DLL_CreateGlobals();

Let's see some methods to achieve coordinate transformations, and some examples of how to manipulate coordinates and frames, using Chrono features.
You can use basic linear algebra, or ChTrasform, or ChCoordsys or ChFrame functions to transform points from/to local coordinates in 3D, in ascending complexity and capabilities.
- The ChTransform functions can be used for basic transformations, as static functions.
- The ChCoordsys<> class correspond to a 'light' object (with small memory footprint) consisting of just a vector (for the translation) and a quaternion (for the rotation).
- The ChFrame<> class is more powerful than the ChCoordsys<>, but at the cost of a larger memory consumption (it includes also a 3x3 matrix to perform transformation of large amounts of point as fast as possible).

Before testing the various methods, we need some data to work on (points, quaternions expressing rotations, etc.). Just some examples:

	
	chrono::Vector mvect2;					// resulting (transformed) vectors will go here
	chrono::Vector mvect3;
						
						// Define a  POINT  to be transformed, expressed in
						// local frame coordinate.
	chrono::Vector mvect1(2,3,4);			

						// Define a vector representin the TRASLATION of the frame
						// respect to absolute (world) coordinates.
	chrono::Vector	   vtraslA(5,6,7);		

						// Define a quaternion representing the ROTATION of the frame
						// respect to absolute (world) coordinates. Must be normalized.
	chrono::Quaternion qrotA(1, 3, 4, 5);	
	qrotA.Normalize();

						// ..Also create a 3x3 rotation matrix [A] from the quaternion
						// (at any time you can use mrotA.Set_A_quaternion(qrotA) );
	chrono::Matrix mrotA(qrotA);
	

						// ..Also create a coordsystem object, representing both 
						// translation and rotation.
	chrono::Coordsys csysA (vtraslA, qrotA);

OK!!! Now we are ready to perform the transformation, like in linear algebra formula v'=t+[A]*v, so that we will obtain the coordinates of mvect1 in absolute coordinates. This can be achieved in many ways. Let's see them.


					// TRASFORM USING ROTATION MATRIX AND LINEAR ALGEBRA
					//   
	mvect2 = vtraslA + mrotA*mvect1;					// like:  v2 = t + [A]*v1  
	GetLog() << mvect2 << " ..using linear algebra, \n";

	
					// TRASFORM USING QUATERNION ROTATION

	mvect2 = vtraslA + qrotA.Rotate(mvect1);
	GetLog() << mvect2 << " ..using quaternion rotation, \n";


					// TRASFORM USING THE ChTrasform STATIC METHODS

	mvect2 = chrono::ChTrasform<>::TrasformLocalToParent(mvect1, vtraslA, mrotA);
	GetLog() << mvect2 << " ..using the ChTrasform- vect and rot.matrix, \n";

	mvect2 = chrono::ChTrasform<>::TrasformLocalToParent(mvect1, vtraslA, qrotA);
	GetLog() << mvect2 << " ..using the ChTrasform- vect and quat, \n";


					// TRASFORM USING A ChCoordys OBJECT

	mvect2 = csysA.TrasformLocalToParent(mvect1); 
	GetLog() << mvect2 << " ..using a ChCoordsys object, \n";

	

					// TRASFORM USING A ChFrame OBJECT

	ChFrame<> mframeA(vtraslA, qrotA);  // or ChFrame<> mframeA(vtraslA, mrotA);

	mvect2 = mframeA.TrasformLocalToParent(mvect1); 
	GetLog() << mvect2 << " ..using a ChFrame object function, \n";

	mvect2 = mvect1 >> mframeA; 
	GetLog() << mvect2 << " ..using a ChFrame '>>' operator, \n";

	mvect2 = mframeA * mvect1; 
	GetLog() << mvect2 << " ..using a ChFrame '*' operator, \n";

Now, what happens if you want to perform multiple transformations along a chain of frames, each expressed in previous' coordinates?
If you are familiar with the Denavitt-Hartemberg notation used in robotics, you know that this is possible by concatenating multiplications between 4x4 matrices... The same effect can be obtained in Chrono::Engine using the ChFrame<> class and its operators, as in following examples, which does not even require the Denavitt-Hartemberg matrices:

	chrono::Vector	   v10 (5,6,7);		
	chrono::Quaternion q10 (1, 3, 4, 5);	q10.Normalize();
	chrono::Matrix m10     (q10);

	chrono::Vector	   v21 (4,1,3);		
	chrono::Quaternion q21 (3, 2, 1, 5);	q21.Normalize();
	chrono::Matrix m21     (q21);
	
	chrono::Vector	   v32 (1,5,1);		
	chrono::Quaternion q32 (4, 1, 3, 1);	q32.Normalize();
	chrono::Matrix m32     (q32);

					// ...with linear algebra: 

	mvect3 =  v10 + m10 * (v21 + m21 * (v32 + m32 * mvect1)); 
	GetLog() << mvect3 << " ..triple trsf. using linear algebra, \n";
	
					// ...with ChFrame '>>' operator or "*" operator 
					// is by far much simplier!

	chrono::ChFrame<> f10 (v10, q10); 
	chrono::ChFrame<> f21 (v21, q21);
	chrono::ChFrame<> f32 (v32, q32);


	mvect3 = mvect1 >> f32 >> f21 >> f10;
	GetLog() << mvect3 << " ..triple vector trsf. with ChFrame '>>' operator, \n";

	mvect3 =  f10 * f21 * f32 * mvect1;
	GetLog() << mvect3 << " ..triple vector trsf. with ChFrame '*' operator, \n";

	ChFrame<> tempf(f10 * f21 * f32);
	mvect3 = tempf * mvect1;
	GetLog() << mvect3 << " ..triple vector trsf. with ChFrame '*' operator, \n";

					// Not only vectors, but also frames can be transformed
					// with ">>" or "*" operators.

	chrono::ChFrame<> f_3 (mvect1);
	chrono::ChFrame<> f_0;
	f_0 = f_3 >> f32 >> f21 >> f10;
	GetLog() << f_0 << " ..triple frame trsf. with ChFrame '>>' operator,  \n";
	
	f_0 = f10 *  f21 *  f32 *  f_3;
	GetLog() << f_0 << " ..triple frame trsf. with ChFrame '*' operator,  \n";

Now test inverse transformations too.
From the low-level to the higher level methods, here are some ways to accomplish this.



					// TRASFORM USING ROTATION MATRIX AND LINEAR ALGEBRA
					//    

	GetLog() << mvect1 << " ..mvect1 \n";
	mvect1 = mrotA.MatrT_x_Vect(mvect2 - vtraslA);		 // like:  v1 = [A]'*(v2-t)
	GetLog() << mvect1 << " ..inv, using linear algebra, \n";

	
					// TRASFORM USING QUATERNION ROTATION

	mvect1 = qrotA.RotateBack(mvect2 - vtraslA);
	GetLog() << mvect1 << " ..inv, using quaternion rotation, \n";


					// TRASFORM USING THE ChTrasform STATIC METHODS

	mvect1 = chrono::ChTrasform<>::TrasformParentToLocal(mvect2, vtraslA, mrotA);
	GetLog() << mvect1 << " ..inv, using the ChTrasform- vect and rot.matrix, \n";

	mvect1 = chrono::ChTrasform<>::TrasformParentToLocal(mvect2, vtraslA, qrotA);
	GetLog() << mvect1 << " ..inv, using the ChTrasform- vect and quat, \n";


					// TRASFORM USING A ChCoordys OBJECT

	mvect1 = csysA.TrasformParentToLocal(mvect2); 
	GetLog() << mvect1 << " ..inv, using a ChCoordsys object, \n";

	

					// TRASFORM USING A ChFrame OBJECT

	mvect1 = mframeA.TrasformParentToLocal(mvect2); 
	GetLog() << mvect1 << " ..inv, using a ChFrame object function, \n";

	mvect1 = mvect2 >> mframeA.GetInverse(); 
	GetLog() << mvect1 << " ..inv, using a ChFrame '>>' operator, \n";

	mvect1 = mframeA.GetInverse() * mvect2; 
	GetLog() << mvect1 << " ..inv, using a ChFrame '*' operator, \n";

	mvect1 = mframeA / mvect2; 
	GetLog() << mvect1 << " ..inv, using a ChFrame '/' operator, \n";

	ChFrame<> mframeAinv (mframeA);
	mframeAinv.Invert();
	mvect1 = mframeAinv * mvect2; 
	GetLog() << mvect1 << " ..inv, using an inverted ChFrame \n";

 
 

							// ... also for inverting chain of transformations...
	
	//mvect3 =  f10 * f21 * f32 * mvect1;				// direct transf..

	mvect1 =  (f10 * f21 * f32).GetInverse() * mvect3;	// inverse transf.
	GetLog() << mvect1 << " ..inv three transf \n";

	mvect1 =  f32.GetInverse() * f21.GetInverse() * f10.GetInverse() * mvect3; 
	GetLog() << mvect1 << " ..inv three transf (another method) \n";

	mvect1 =  mvect3 >> (f32 >> f21 >> f10).GetInverse();	
	GetLog() << mvect1 << " ..inv three transf (another method) \n";

	mvect1 =  mvect3 >> f10.GetInverse() >> f21.GetInverse() >> f32.GetInverse();
	GetLog() << mvect1 << " ..inv three transf (another method) \n";

Before exiting from the main() function, terminate the example by printing something onto the console and by deleting global data which the Chrono::Engine library may have created:

	
	GetLog() << "\n  CHRONO execution terminated.";
	

	DLL_DeleteGlobals();

	return 0;
}