How does B3D set the quaternion rotation?

Blitz3D Forums/Blitz3D Programming/How does B3D set the quaternion rotation?

PacMani(Posted 2014) [#1]
Hi there,

Blitz3D uses a quaternion internally to handle the rotation of meshes and cameras.

I was able to figure out how it retrieves Pitch Yaw and Roll from the quaternion with the help of Marks code which he has pinned to this subforum.

I just don't get it how a call like RotateEntity(entity, pitch#, yaw#, roll#) creates the quaternion again.

I tried to do it on my own but the results are completely wrong angles and weirdly spinning entities / cameras.

E.g., I don't know how to set an entities own rotation, like: RotateEntity(entity, EntityPitch(entity), EntityYaw(entity), EntityRoll(entity)

How is it done mathematically? Any help?


fox95871(Posted 2014) [#2]
No idea, but I did once spend nine months of my precious life overriding it. Basically, I just made it so absolutely every vertex on a mesh was being constantly tracked by a data file. In effect, when I said turn, it turned, and when I said tilt, it tilted. It even called me sir at one point I think, but I may have been tired. Anyway, I got it so consecutive adjustments to the yaw pitch and roll responded exactly the way they do in Maya... no easy feat!


Kryzon(Posted 2014) [#3]
In other words, you want to create a quaternion rotation from an Euler rotation. The simplest solution seems to be this, creating one quaternion for each axis, and multiplying them together:

http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation#Quaternion_from_Euler_angles

You may also want to use the following Blitz3D command: GetMatElement()


Yasha(Posted 2014) [#4]
If it helps, here is some code that converts an Euler rotation to a quaternion, and extracts individual Euler components back from a quaternion:



(In the last one, the quaternion is returned by modifying the q[] array.) The functions are from bOGL-2, a complete 3D engine implemented only in Blitz Basic code (in case you want to see how some of the other things are done too).

There's a good explanation of quaternion operations here, which is useful: http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/index.htm


PacMani(Posted 2014) [#5]
I tried your solutions you've posted, and Yasha's answer came very close to be what I wanted - but it had the same issue as another solution I've tried before:

If I only pitch / yaw or roll my camera, everything's fine, but as soon as I add another rotation to it (e.e.g pitch AND yaw), it starts to spin weirdly again around whatever direction that might be.

Kryzon's solution got me completely unexplainable results and the camera just went crazy ;/

At least I got the euler angles from the Quaternion now correctly, as done in B3D (C# code):
public static class QuaternionExtensions
{        
    public static Vector3 GetPitchYawRoll(this Quaternion q)
    {
        return new Vector3(q.GetPitch(), q.GetYaw(), q.GetRoll());
    }

    public static float GetPitch(this Quaternion q)
    {
        return q.GetK().GetPitch();
    }

    public static float GetYaw(this Quaternion q)
    {
        return q.GetK().GetYaw();
    }

    public static float GetRoll(this Quaternion q)
    {
        // This is M12 * M22 of rotation matrix
        float xx = q.X * q.X;
        float xy = q.X * q.Y;
        float zz = q.Z * q.Z;
        float wz = q.W * q.Z;
        return (float)Math.Atan2(2f * (xy - wz), 1f - 2f * (xx + zz));
    }

    public static Vector3 GetK(this Quaternion q)
    {
        float xz = q.X * q.Z;
        float wy = q.W * q.Y;
        float yz = q.Y * q.Z;
        float wx = q.W * q.X;
        float xx = q.X * q.X;
        float yy = q.Y * q.Y;
        return new Vector3(
            2f * (xz - wy),
            2f * (yz + wx),
            1f - 2f * (xx + yy));
    }

}

public static class Vector3Extensions
{
    public static float GetPitch(this Vector3 v)
    {
        return (float)-Math.Atan2(v.Y, Math.Sqrt(v.X * v.X + v.Z * v.Z));
    }

    public static float GetYaw(this Vector3 v)
    {
        return (float)-Math.Atan2(v.X, v.Z);
    }
}

Now what the remaining problem is, if someone sets back the Vector3 he got from GetPitchYawRoll (containing Pitch in X, Yaw in Y and Roll in Z) and wants the quaternion from it here:
public Vector3 Rotation
{
    get
    {
        return _rotation.GetPitchYawRoll();
    }
    set
    {
        // Whoever gives me the solution here wins some PayPal
        UpdateViewMatrix();
    }
}

EDIT: Oh my, maybe my error is in the UpdateViewMatrix function? I don't see it as it seems pretty simple for me, but it might be wrong:
private void UpdateViewMatrix()
{
    Matrix rotation = Matrix.RotationQuaternion(_rotation);
    Matrix translation = Matrix.Translation(-_position.X, -_position.Y, -_position.Z);
    _view = translation * rotation;
}

That Matrix.RotationQuaternion function is from SharpDX - and I learned not to trust those functions in Quaternion stuff... What it does is this (decompiled code, I simplified the variable names):
public static void RotationQuaternion(ref Quaternion rotation, out Matrix result)
{
    float xx = rotation.X * rotation.X;
    float yy = rotation.Y * rotation.Y;
    float zz = rotation.Z * rotation.Z;
    float xy = rotation.X * rotation.Y;
    float zw = rotation.Z * rotation.W;
    float zx = rotation.Z * rotation.X;
    float yw = rotation.Y * rotation.W;
    float yz = rotation.Y * rotation.Z;
    float xw = rotation.X * rotation.W;
    result = Matrix.Identity;
    result.M11 = 1f - 2f * (yy + zz);
    result.M12 = 2f * (xy + zw);
    result.M13 = 2f * (zx - yw);
    result.M21 = 2f * (xy - zw);
    result.M22 = 1f - 2f * (zz + xx);
    result.M23 = 2f * (yz + xw);
    result.M31 = 2f * (zx + yw);
    result.M32 = 2f * (yz - xw);
    result.M33 = 1f - 2f * (yy + xx);
}


EDIT2: I even tried converting the B3D code where the Matrix constructor accepted a quaternion and used that one as a rotation matrix, but it doesnt work too.

EDIT3: I posted a StackOverflow question: http://stackoverflow.com/questions/23310299/quaternion-from-tait-bryan-angles


Kryzon(Posted 2014) [#6]
There's a difference in your rotation matrix constructor to Mark's. Some signs are different.
	Matrix( const Quat &q ){
		float xx=q.v.x*q.v.x,yy=q.v.y*q.v.y,zz=q.v.z*q.v.z;
		float xy=q.v.x*q.v.y,xz=q.v.x*q.v.z,yz=q.v.y*q.v.z;
		float wx=q.w*q.v.x,wy=q.w*q.v.y,wz=q.w*q.v.z;
		i=Vector( 1-2*(yy+zz),2*(xy-wz),2*(xz+wy) ),
		j=Vector( 2*(xy+wz),1-2*(xx+zz),2*(yz-wx) ),
		k=Vector( 2*(xz-wy),2*(yz+wx),1-2*(xx+yy) );
	}
With i, j and k being Vec4 as the matrix rows.

EDIT: I also don't understand why you're computing the pitch, yaw and roll out of the K term of the quaternion. Isn't K just an imaginary term?
The correct way to retrieve the individual angles should be done from the orientation vector of the quaternion (q.x, q.y and q.z), with rotation q.w.
	float Pitch() {
		return -asinf( 2*q.z*q.y+2*q.x*q.w );
	}

	float Yaw() {
		float x2=q.x*q.x, y2=q.y*q.y;
		return atan2f( 2*q.y*q.w-2*q.z*q.x,1-2*y2-2*x2 );
	}

	float Roll() {
		float x2=q.x*q.x, z2=q.z*q.z;
		return -atan2f( 2*q.z*q.w-2*q.y*q.x,1-2*z2-2*x2 );
	}



Floyd(Posted 2014) [#7]
If you really have to match Blitz3D then here are some things to keep in mind.

The term Euler Angles refers to any system which does three turns around the X,Y,Z axes with the second and third turns being done around an axis as it is orientated after the previous turn(s).

There are actually twelve ways this can be done. There are three choices for the first axis. Then two choices for the second and third since they can be anything other than the axis used for the previous turn. Any of these could be used to build a working system.

Blitz3D uses the sequence Y-X-Z.

Quaternion [w,x,y,z] represents a turn around vector [x,y,z] through an angle represented by w, which is the cosine of half the angle. When regarded as a 4-d vector the quaternion must have length 1.

As an example, consider a rotation of 120 degrees. Cos(60) is exactly 1/2 so any such quaternion must have the form [0.5,x,y,z].

Let's try a turn around the X-axis. Without the 'length 1' restriction we could use quaternion [0.5, 1,0,0].
But in fact we must use [0.5, Sqr(0.75),0,0], where 0.75 is 1 - (0.5)^2.

Rotations around Y or Z are similarly easy. The general case is done by concatenating, via quaternion multiplication, the simple Y,X,Z turns. That's easy enough for specific numerical cases. But if you want to work out general equations expressing x,y,z in terms of the three angles it is quite a chore. That's best done with some symbolic mathematics software.

There are still some potential headaches. Blitz3D uses left handed coordinates so one of these turns is going the opposite way from what you might expect. By observing Blitz3D in action it seems that Y-turns are going the opposite way. What I mean is that when looking down the Y+ axis a small positive turn around the axis will be clockwise. For X and Z it is counterclockwise.

Also notice that every rotation has two quaternion representations. Suppose you and I face each other, looking at a wheel turning in the vertical plane separating us. If you see a clockwise turn then I see counterclockwise. That means that making the axis point the opposite way, and likewise the turn being opposite, represents the same physical rotation, hence two quaternions. You would have to reverse engineer whatever rule Blitz3D is using to choose between then.

Finally, there is gimbal lock, with pitch being +90 or -90. That's another adventure. And keep in mind that a robust system doesn't just have to deal with theoretically bad cases. It must somehow handle those which are which are very close to bad although not exactly so.

You will need some perseverance to get through all this.


PacMani(Posted 2014) [#8]
@Kryzon: Thanks for the corrections in getting pitch yaw and roll, they work like a charm.
However, I don't see the wrong signs in my rotation matrix method? :o I changed the order of calculating the multiplicated values, but they're still the same. I'm assigning the values in the order M11, M12, M13, M14, M21, M22 etc., which would be the same order as in the 3 matrix vectors.

@Floyd: Thanks for the explanations. After browsing so many sources (some of them simply being wrong at what I found out...) I already got headache for real... and I'm not sure if I will ever get this working correctly.

I am using a LH coordinate system too.

I noticed that if I turn a camera into 30 degrees around the Y axis in a simple B3D test program, it turns counter-clockwise?! I always thought it's clockwise... I'm completely messed up and in a dead-end.
I tried about twenty solutions now but every time I combined an axis with another one it messed up.


Well... since my headache isn't getting less at the moment :( If anyone wants to look, here's the whole C# source code project: http://vibeware.net/_pacmani/Vibeware.Spark.zip and an executable for you to test (shows the rotation quaternion values before setting it back to itself in the console): http://vibeware.net/_pacmani/Executable.zip

If anyone can give me a solution... I hope I can donate him some money or whatever I might buy for him.


Floyd(Posted 2014) [#9]
Here's something of a blind attempt, making necessary changes to somebody else's code.
The results look reasonable, but might still need a little tinkering.

; Euler to/from Quaternion, adapted from
; www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
; That site uses heading to mean yaw, attitude for pitch, bank for roll
; It also interchanges the meaning of x and z and uses a right handed system.

Type Rotation
	Field pitch#, yaw#, roll#
End Type

Type Quat
	Field w#, x#, y#, z#
End Type


Function RotationToQuaternion( q.Quat, r.Rotation )

	Local c1#,c2#,c3#
	Local s1#,s2#,s3#
	Local w#,x#,y#,z#
	
	c1 = Cos(r\yaw/2)
	c2 = Cos(r\pitch/2)
	c3 = Cos(r\roll/2)
	
	s1 = Sin(r\yaw/2)
	s2 = Sin(r\pitch/2)
	s3 = Sin(r\roll/2)
	
	; x,z swapped and z made negative.
	
	q\w = c1*c2*c3 - s1*s2*s3
	q\z = s1*s2*c3 + c1*c2*s3  :  z = -z
	q\y = s1*c2*c3 + c1*s2*s3
	q\x = c1*s2*c3 - s1*c2*s3
	
End Function

Function QuaternionToRotation( r.Rotation, q.Quat )

	Local test#, sqx#,sqy#,sqz#

	test = q\z*q\y + q\x*q\w
	If test > 0.499	; singularity at north pole, 0.499 means about 86.3 degrees
		r\yaw = 2 * ATan2(q\z,q\w)
		r\pitch = 180
		r\roll = 0
		Return
	End If
	If (test < -0.499) ; singularity at south pole
		r\yaw = -2 * ATan2(q\z,q\w)
		r\pitch = -180
		r\roll = 0
		Return
	End If

	sqz = q\z*q\z
	sqy = q\y*q\y
	sqx = q\x*q\x
	r\yaw = ATan2(2*q\y*q\w-2*q\z*q\x , 1 - 2*sqy - 2*sqx)
	r\pitch = ASin(2*test)
	r\roll = ATan2(2*q\z*q\w-2*q\y*q\x , 1 - 2*sqz - 2*sqx)
	
End Function



Kryzon(Posted 2014) [#10]
Also try this different QuaternionExtensions code:



The function YawPitchRollQuat() generates a quaternion from a Vector3, with each member of the vector set to the desired yaw, pitch and roll values.


PacMani(Posted 2014) [#11]
The code looks good so far, some questions from me, the camera is still behaving weirdly:

- It says right-handed coordinate system in the description at the top, aren't we using an LH system in Blitz / DX?
- I had to remove the negation of Z to get the roll working for me (otherwise it always jumped around from a negative Z value to a positive Z value when trying to rotate each time).
- Aren't we using radians? How does the r\pitch = -180 line come to its meaning then? ;o

Sorry for being so picky and nasty :S

Here's the code I converted it to so far, and the current executable with exactly your code is uploaded here (rotate with WASD and Home/End): http://www.vibeware.net/_pacmani/Release.zip
Notice how roll doesn't work here (I left the negation in the executable) and the camera moves jumpy when rotating around more than one axis.
internal static Vector3 GetPitchYawRoll(this Quaternion q)
{
    Vector3 rotation = new Vector3();

    double test = q.Z * q.Y + q.X * q.W;
    if (test > 0.499)
    {
        // Singularity at north pole, 0.499 means about 86.3 degrees
        rotation.Y = (float)(2.0 * Math.Atan2(q.Z, q.W));
        rotation.X = 180f;
    }
    else if (test < -0.499)
    {
        // Singularity at south pole
        rotation.Y = (float)(-2.0 * Math.Atan2(q.Z, q.W));
        rotation.X = -180f;
    }
    else
    {
        double zSquare = q.Z * q.Z;
        double ySquare = q.Y * q.Y;
        double xSquare = q.X * q.X;
        rotation.Y = (float)Math.Atan2(2.0 * q.Y * q.W - 2.0 * q.Z * q.X,
            1.0 - 2.0 * ySquare - 2f * xSquare);
        rotation.X = (float)Math.Asin(2.0 * test);
        rotation.Z = (float)Math.Atan2(2.0 * q.Z * q.W - 2.0 * q.Y * q.X,
            1.0 - 2.0 * zSquare - 2.0 * xSquare);
    }

    return rotation;
}

internal static Quaternion RotationQuaternion(this Vector3 v)
{
float c1 = (float)Math.Cos(v.Y / 2f);
float c2 = (float)Math.Cos(v.X / 2f);
float c3 = (float)Math.Cos(v.Z / 2f);
float s1 = (float)Math.Sin(v.Y / 2f);
float s2 = (float)Math.Sin(v.X / 2f);
float s3 = (float)Math.Sin(v.Z / 2f);

return new Quaternion(
    c1 * s2 * c3 - s1 * c2 * s3, //X
    s1 * c2 * c3 + c1 * s2 * s3, //Y
    -(s1 * s2 * c3 + c1 * c2 * s3), //Z
    c1 * c2 * c3 - s1 * s2 * s3); //W
}


The whole code can be found here again: http://www.vibeware.net/_pacmani/Vibeware.Spark.zip


Kryzon(Posted 2014) [#12]
Is the YawPitchRollQuat() method working?


PacMani(Posted 2014) [#13]
Oh sorry Kryzon, I didn't see your new post while trying out the previous answer.

Well... wow! That is amazing, it works perfectly! Absolutely perfectly... I think you're the winner here or something, lol.

Is it really required to have the yaw as the first component in the vector? Because I find Vector.X = Yaw a bit irritating. I can simply switch it can't I (bit of a dumb question but just making sure I don't mess up again)?


Kryzon(Posted 2014) [#14]
The quaternion sequence needs to be Y-X-Z, like Floyd said, but you certainly don't need to input the values in that same order.

You can just swizzle the values like this, which must be what Blitz3D does:
	internal static Quaternion PitchYawRollQuat(this Vector3 v)
	{
		Quaternion r;

		float pitch = v.X;
		v.X = v.Y; // Swapping the pitch and yaw.
		v.Y = pitch;

		float c1=cosf(-v.Z/2),s1=sinf(-v.Z/2);
		float c2=cosf(-v.Y/2),s2=sinf(-v.Y/2);
		float c3=cosf( v.X/2),s3=sinf( v.X/2);
		float c1_c2=c1*c2,s1_s2=s1*s2;
		r.X=c1*s2*c3-s1*c2*s3;
		r.Y=c1_c2*s3+s1_s2*c3;
		r.Z.z=s1*c2*c3+c1*s2*s3;
		r.W=c1_c2*c3-s1_s2*s3;
		return r;
	}



PacMani(Posted 2014) [#15]
That was what really confused me at first place, when it was said it's in YXZ order and B3D does some other kind of order. Well now I got its simple "component swizzling" ;) Thanks so much again. I wrote you an email about the "reward" ;)

EDIT: ah well, found a weird problem: if I rotate my camera around 90° upwards (pitch) and play around at this height, I think some rounding errors come in affect and cause all values to become NaN :S what to do?
EDIT2: I solved this problem: The calculation result of the parameter to Math.Asin in GetPitch was sometimes a tiny bit larget than 1.0 (about 1.0000002) and thus NaN was returned. Simply normalizing the returned quaternion before didn't help, the calculation itself created a result which was too large. I clamped it to max. 1.0 now.


fox95871(Posted 2014) [#16]
Let me know if you ever want the code I described. I won't accept a reward, though.


PacMani(Posted 2014) [#17]
I am currently using Kryzon's code, it's working exactly. Thanks for your help though. If I have any questions again, I'll let you know for a rewarding answer :D