3d transform

[x-alberto]

Sometimes you need to perform cumbersome math just to to put an object into the right 3D place or, as for my specific case, to get the right relative position of two objects (from two different aircrafts).

Suppose you need to know the coordinates of a point of a given aircraft (e.g.because you are actually going to draw an object in that precise point)

What you have is the aircraft position (it's geometric center in x-plane local coordinates + heading, roll and pitch) and the offset of your point in aircraft coordinates.

what you need is the point position (x,y,z in x-plane local coordinates)

Transform3D (code below) will take as input:

inLocalCoords: the offest of your object in acf coordinates

inMap: the coordinates and angles of the aircraft

and will pop out:

outCoords: the coordinates of your object in x.plane "local" coordinates

(Bottom line:

Unless it's already there, this should really go into Gizmo)

void Transform3D(const double inLocalCoords[3], /*acf coordinates*/
	                  const double inMap[6],   /*x, y, z, heading, pitch, roll (X-Plane has angles in degrees)*/ 
					  double       outCoords[3]){


	//http://en.wikipedia.org/wiki/Rotation_matrix
	//http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/ArbitraryAxisRotation.html

	//Make constants easier to handle. 
	//Put angles into radians.
	const double H_rad = -inMap[3]*DEG2RAD; //heading
	const double P_rad =  inMap[4]*DEG2RAD; //pitch
	const double R_rad = -inMap[5]*DEG2RAD; //roll

	const double cos_H = cos(H_rad);
	const double sin_H = sin(H_rad);
	const double cos_P = cos(P_rad);
	const double sin_P = sin(P_rad);
	const double cos_R = cos(R_rad);
	const double sin_R = sin(R_rad);

	//Pitch rotation matrix
	const double Mx[3][3] = {
	  {      1,       0,       0  },
	  {      0,   cos_P,  -sin_P  },
	  {      0,   sin_P,   cos_P  }
	};

	//Heading rotation matrix
	const double My[3][3] = {
	  {  cos_H,       0,   sin_H  },
	  {      0,       1,       0  },
	  { -sin_H,       0,   cos_H  }
	};

	//Roll rotation matrix
	const double Mz[3][3] = {
	  {  cos_R,  -sin_R,       0  },
	  {  sin_R,   cos_R,       0  },
	  {      0,       0,       1  }
	};

	//What we want is out = [Mx*My*Mz] * in
	//We will use three consequent multiplications
	//The _only_ correcy sequence is 
	//Mz 
	//Mx 
	//My
	//otherwise you get garbage (see multiplication rules for matrices...)

	double step1[3];
	double step2[3];

	// step1 = Mz * in
	mmult(inLocalCoords, Mz, step1);

	// step2 = Mx * step1
	mmult(step1,         Mx, step2);
    
	// out = My * step2
	mmult(step2,         My, outCoords);

	/*Finally, apply translation*/
	for(int i=0; i<3; i++)
		outCoords[i]+=inMap[i]; /* X, Y, Z*/
}

void mmult(const double inX[3], const double R[3][3], double outX[3]){

	for(int n=0; n<3; n++)
		outX[n]=0;

	for(int i=0; i<3; i++)
		for(int j=0; j<3; j++)
			outX[i] += R[i][j] * inX[j];

}

edited: using the code tag a few missing indices just popped out again

[Ben Russell]

thank you! again!

I've been meaning to dig this out of the Mesa libraries, you just saved me a whole heap of pain. Again.

Thanks!

[x-alberto]

thank you! again!

I've been meaning to dig this out of the Mesa libraries, you just saved me a whole heap of pain. Again.

Thanks!

glad to be helpful

:-)

[Nik]

Good to see 2 great devs talking togheter  8)

[x-alberto]

Good to see 2 great devs talking togheter  8)

Hi Nik, you here?

Actually Indi's job is (still) a few miles ahead of mine....

[x-alberto]

If you prefer a more compact code (possibly less readable), here is the resulting transformation matrix managing the three rotations  in one single step:

	const double Myxz[3][3] = {
		{ cos_H*cos_R + sin_H*sin_P*sin_R,	-cos_H*sin_R + sin_H*sin_P*cos_R,	sin_H*cos_P},
		{                     cos_P*sin_R,	                     cos_P*cos_R,	     -sin_P},
		{-sin_H*cos_R + cos_H*sin_P*sin_R,	 sin_H*sin_R + cos_H*sin_P*cos_R,	cos_H*cos_P},
    };

[Ben Russell]

 transformed_x,transformed_y,transformed_z = matrix.transform3D( x,y,z, p,r,h )

This new API has been added to Gizmo tonight, it will be available in the next version of Gizmo. ( > 11.3.15 )

[x-alberto]

 transformed_x,transformed_y,transformed_z = matrix.transform3D( x,y,z, p,r,h )

I agree, one can easily apply the final translation in his own code...

[PhM]

Hi x-alberto,

By using structures to clearly define what is handled by your routines you will benefit from efficient type checking at compile time that will filter out several possible mistakes. It also happens to be helping the compiler out quite a bit as the code generated is over 2.5 times smaller, thus faster as less instructions get executed.

I guess that you already know that if you are considering transforming more than a few vectors you should consider using SSE instructions, along with the compound transformation matrix that you mentioned that also saves a lot of instructions.

Here is what I came up with, feel free to do whatever you may want with this code.

PhM

struct Vector3
{
	double x;
	double y;
	double z;
};

struct Inputs
{
	Vector3	Coordinates;
	Vector3	Rotations;
};

struct Matrix3x3
{
	Vector3	Row1;
	Vector3	Row2;
	Vector3	Row3;
};

void Transform3D(const Vector3	*in_pLocalCoords,
			const Inputs	*in_pMap,
			Vector3		*out_pCoords)
{
	//http://en.wikipedia.org/wiki/Rotation_matrix
	//http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/ArbitraryAxisRotation.html

	//Make constants easier to handle.
	//Put angles into radians.
	const double H_rad = -in_pMap->Rotations.x * DEG2RAD; //heading
	const double P_rad =  in_pMap->Rotations.y * DEG2RAD; //pitch
	const double R_rad = -in_pMap->Rotations.z * DEG2RAD; //roll

	const double cos_H = cos(H_rad);
	const double sin_H = sin(H_rad);
	const double cos_P = cos(P_rad);
	const double sin_P = sin(P_rad);
	const double cos_R = cos(R_rad);
	const double sin_R = sin(R_rad);

	//Pitch rotation matrix
	const Matrix3x3 Mx =
	{
		    1.0,       0.0,       0.0,
		    0.0,     cos_P,    -sin_P,
		    0.0,     sin_P,     cos_P
	};

	//Heading rotation matrix
	const Matrix3x3 My =
	{
		  cos_H,       0.0,   sin_H,
		    0.0,       1.0,     0.0,
		 -sin_H,       0.0,   cos_H
	};

	//Roll rotation matrix
	const Matrix3x3 Mz =
	{
		  cos_R,  -sin_R,     0.0,
		  sin_R,   cos_R,     0.0,
		    0.0,     0.0,     1.0
	};

	//What we want is out = [Mx*My*Mz] * in
	//We will use three consequent multiplications
	//The _only_ correcy sequence is 
	//Mz 
	//Mx 
	//My
	//otherwise you get garbage (see multiplication rules for matrices...)

	Vector3 step1	= {0.0,0.0,0.0};
	Vector3 step2	= {0.0,0.0,0.0};

	// step1 = Mz * in
	mmult(in_pLocalCoords, &Mz, &step1);

	// step2 = Mx * step1
	mmult(&step1, &Mx, &step2);

	// out = My * step2
	mmult(&step2, &My, out_pCoords);

	/*Finally, apply translation*/
	out_pCoords->x += in_pMap->Coordinates.x;
	out_pCoords->y += in_pMap->Coordinates.y;
	out_pCoords->z += in_pMap->Coordinates.z;
}

void mmult(const Vector3 *in_pX, const Matrix3x3 *in_pR, Vector3 *out_pX)
{
	out_pX->x	 = in_pR->Row1.x * in_pX->x;
	out_pX->x	+= in_pR->Row1.y * in_pX->y;
	out_pX->x	+= in_pR->Row1.z * in_pX->z;

	out_pX->y	 = in_pR->Row2.x * in_pX->x;
	out_pX->y	+= in_pR->Row2.y * in_pX->y;
	out_pX->y	+= in_pR->Row2.z * in_pX->z;

	out_pX->z	 = in_pR->Row3.x * in_pX->x;
	out_pX->z	+= in_pR->Row3.y * in_pX->y;
	out_pX->z	+= in_pR->Row3.z * in_pX->z;
}

[x-alberto]

Hi PhM,

thank you for your comments and for the code.

I will try to build a function using both your suggestions and  the compound rotation matrix.

I will investigate using SSE  - do I understand corretly that this will require a small ASM fragment?

Cheers

Alberto

[PhM]

I will investigate using SSE  - do I understand correctly that this will require a small ASM fragment?

Hi,

Depends, Do you support multiple platforms ? If yes then  what compiler are you using on each platform ?

MS's compiler provides intrinsics for these instructions, I read things that lead me to believe that they are also available in XCode. There is some documentation on MSDN, look for streaming SIMD extensions. Once wrapped properly you can have the same syntax used for every platform.

These instructions will do miracle provided that you have enough stuff to be processed, otherwise the improvement versus floating point is less.

PhM

[x-alberto]

Depends, Do you support multiple platforms ? If yes then  what compiler are you using on each platform ?

Hi

I am currently using visual c for win, x-code for mac and gcc for lin. (I definitely see a point in using gizmo instead!)

For my ongoing projects the number of transforms per frame is very limited so I will not probably move from my current implementation.

In the near future, however, optimisation might matter and, moreover, learning new tricks is cool.

I will follow your advice and will post my findings.

Ciao

Alberto