Quaternions Test

Blitz3D Forums/Blitz3D Programming/Quaternions Test

Bobysait

(Posted 2006) [#1]

Here are some functions found on the forum, and modified for easier use.

But, I have a problem with the conversions, axes seem to be false.
As, i 'm not really understanding quaternions, i really think i made a mistake, but i can't point on this...
If anyone could help ^^

Graphics3D 800,600,0,2
SetBuffer BackBuffer()

;demo
QUA_EulerToQuat(45,0,0)
	w1#=QUA_Get_QuatW()
	x1#=QUA_Get_QuatX()
	y1#=QUA_Get_QuatY()
	z1#=QUA_Get_QuatZ()

QUA_EulerToQuat(45,0,0)
	w2#=QUA_Get_QuatW()
	x2#=QUA_Get_QuatX()
	y2#=QUA_Get_QuatY()
	z2#=QUA_Get_QuatZ()

QUA_MultiplyQuat#(w1,x1,y1,z1,w2,x2,y2,z2)
	w#=QUA_Get_QuatW()
	x#=QUA_Get_QuatX()
	y#=QUA_Get_QuatY()
	z#=QUA_Get_QuatZ()
Color 0,255,0
Print " ===== Quat Multiply ====="
Print " 2 * Conversion de (45,0,0) en Quat"
Print " puis multiplication des deux Quats pour ajouter les angles d'Euler"
Print
Color 255,255,0:Print "rw="+w+" - rx="+x+" - ry="+y+" - rz="+z:Print:Print

Color 0,255,0
Print " ===== Quat to Euler ===== ":Print "=============================================="
Write " => methode 1 :"
QUA_QuatToEuler1(w,x,y,z)
	xf#=QUA_Get_EulerX()
	yf#=QUA_Get_EulerY()
	zf#=QUA_Get_EulerZ()
Color 255,255,0
Print "      rx="+xf
Color 255,0,0
Print "                     ry="+yf+" <= mauvais axe..."
Color 255,255,0
Print "                     rz="+zf:Print
Color 0,255,0:Print "=================================="
Write " => methode 2 :"
QUA_QuatToEuler2(w,x,y,z)
	xf#=QUA_Get_EulerX()
	yf#=QUA_Get_EulerY()
	zf#=QUA_Get_EulerZ()
Color 255,255,0
Print "      rx="+xf+" <= ... chelou non ?"
Color 255,0,0
Print "                     ry="+yf+" <= mauvais axe..."
Color 255,255,0
Print "                     rz="+zf+" <= ... idem que pour X :-s":Print
Color 0,255,0:Print "=================================="
Write " => methode 3 :"
QUA_QuatToEuler3(w,x,y,z)
	xf#=QUA_Get_EulerX()
	yf#=QUA_Get_EulerY()
	zf#=QUA_Get_EulerZ()
Color 255,255,0
Print "      rx="+xf
Color 255,0,0
Print "                     ry="+yf+" <= mauvais axe..."
Color 255,255,0
Print "                     rz="+zf:Print
Color 0,255,0:Print "=============================================="
WaitKey
End




Const QuatToEulerAccuracy# = 0.001
Const QuatSlerpAccuracy#   = 0.0001

Type QUA_Euler
	Field x#,y#,z#
	Field roll#,pitch#,yaw#
End Type

Type QUA_Quat
	Field w#
	Field x#
	Field Y#
	Field z#
End Type

Type QUA_matrix
	Field m00#
	Field m01#
	Field m02#
	Field m03#
	Field m10#
	Field m11#
	Field m12#
	Field m13#
	Field m20#
	Field m21#
	Field m22#
	Field m23#
	Field m30#
	Field m31#
	Field m32#
	Field m33#
End Type



Function QUA_Get_EulerX#()
	eul.QUA_Euler	=	Last QUA_Euler
	If eul=Null Return False
	Return eul\x
End Function

Function QUA_Get_EulerY#()
	eul.QUA_Euler	=	Last QUA_Euler
	If eul=Null Return False
	Return eul\y
End Function

Function QUA_Get_EulerZ#()
	eul.QUA_Euler	=	Last QUA_Euler
	If eul=Null Return False
	Return eul\z
End Function



Function QUA_Get_QuatW#()
	qua.QUA_Quat	=	Last QUA_Quat
	If qua=Null Return False
	Return qua\w
End Function

Function QUA_Get_QuatX#()
	qua.QUA_Quat	=	Last QUA_Quat
	If qua=Null Return False
	Return qua\x
End Function

Function QUA_Get_QuatY#()
	qua.QUA_Quat	=	Last QUA_Quat
	If qua=Null Return False
	Return qua\y
End Function

Function QUA_Get_QuatZ#()
	qua.QUA_Quat	=	Last QUA_Quat
	If qua=Null Return False
	Return qua\z
End Function


Function QUA_EulerToQuat(npitch#,nyaw#,nroll#)
		
	Local cr#,cp#,cy#
	Local sr#,sp#,sy#
	Local cpcy#,spyc#
	Local spcy#,cpsy#
	Local spsy#
	cr#=Cos(-nroll#/2.0)
	cp#=Cos(npitch#/2.0)
	cy#=Cos(nyaw#/2.0)
	sr#=Sin(-nroll#/2.0)
	sp#=Sin(npitch#/2.0)
	sy#=Sin(nyaw#/2.0)
	cpcy#=cp#*cy#
	spsy#=sp#*sy#
	spcy#=sp#*cy#
	cpsy#=cp#*sy#

	q.QUA_Quat=Last QUA_Quat
	If q=Null q=New QUA_Quat

	q\w=cr#*cpcy#+sr#*spsy#
	q\x=sr#*cpcy#-cr#*spsy#
	q\y=cr#*spcy#+sr#*cpsy#
	q\z=cr#*cpsy#-sr#*spcy#

End Function
	
	
	
Function QUA_QuatToEuler1(w#,x#,y#,z#)
	lx#	=	-z
	ly#	=	-x
	lz#	=	y
	x=lx
	y=ly
	z=lz
	Local sint#=(2.0*w*y)-(2.0*x*z)
	Local cost_temp#=1.0-sint*sint
	Local cost#
	If Abs(cost_temp#)>0.001
		cost#=Sqr(cost_temp)
	Else
		cost#=0.0
	EndIf
	Local sinw#,cosw#,sinf#,cosf#,sinv#,cosv#
	If Abs(cost#)>0.001
		sinv#=((2.0*y*z)+(2.0*w*x))/cost
		cosv#=(1.0-(2.0*x*x)-(2.0*y*y))/cost
		sinf#=((2.0*x*y)+(2.0*w*z))/cost
		cosf#=(1.0-(2.0*y*y)-(2.0*z*z))/cost
	Else
		sinv=(2.0*w*x)-(2.0*y*z)
		cosv=1.0-(2.0*x*x)-(2.0*z*z)
		sinf=0.0
		cosf=1.0
	EndIf
	eul.QUA_Euler	=	Last QUA_Euler
	If eul=Null
		eul.QUA_Euler	=	New QUA_Euler
	EndIf

	eul\z#=-ATan2(sinv,cosv)
	eul\x#=ATan2(sint,cost)
	eul\y#=ATan2(sinf,cosf)
End Function



Function QUA_QuatToEuler2(w#,x#,y#,z#)
	q.QUA_Quat=Last QUA_Quat
	If q=Null q=New QUA_Quat
	q\w=w:q\x=x:q\y=y:q\z=z
	Local lx#, ly#, lz#
	eul.QUA_Euler	=	Last QUA_Euler
	If eul=Null
		eul.QUA_Euler	=	New QUA_Euler
	EndIf
	lMat.QUA_matrix = New QUA_matrix
	QUA_QuatToMatrix(q,lMat)
	ly	=	lMat\m02
	If (Abs(ly) > 1.0)
		If  ly > 0.0
			ly =  1.0
		Else
			ly = -1.0
		EndIf
	EndIf
	ly	=	ASin(ly)
	If radian
		lx = ATan2(-lMat\m12, lMat\m22)
		lz = ATan2(-lMat\m01, lMat\m00)
		eul\x = lx
		eul\y = ly
		eul\z = lz
	Else
		If (ly < 90 ) Then
			If (ly > -90) Then
				lx = ATan2(-lMat\m12, lMat\m22)
				lz = ATan2(-lMat\m01, lMat\m00)
			Else
				lx = -ATan2(lMat\m10, lMat\m11)
				lz = 0.0
		   EndIf
		Else
			lx = ATan2(lMat\m10, lMat\m11)
			lz = 0.0
		EndIf
		eul\x = lx ; pitch
		eul\y = ly ; yaw
		eul\z = lz ; roll
	EndIf
	Return True
End Function




; convert a Quat to a Rotation
Function QUA_QuatToEuler3(w#,x#,y#,z#)
	Local sint#, cost#, sinv#, cosv#, sinf#, cosf#
	Local cost_temp#
	sint = (2 * w * y) - (2 * x * z)
	cost_temp = 1.0 - (sint * sint)
	If Abs(cost_temp) > QuatToEulerAccuracy
		cost = Sqr(cost_temp)
		Else
		cost = 0
		EndIf
	If Abs(cost) > QuatToEulerAccuracy
		sinv = ((2 * y * z) + (2 * w * x)) / cost
		cosv = (1 - (2 * x * x) - (2 * y * y)) / cost
		sinf = ((2 * x * y) + (2 * w * z)) / cost
		cosf = (1 - (2 * y * y) - (2 * z * z)) / cost
		Else
		sinv = (2 * w * x) - (2 * y * z)
		cosv = 1 - (2 * x * x) - (2 * z * z)
		sinf = 0
		cosf = 1
		EndIf

	eul.QUA_Euler	=	Last QUA_Euler
	If eul=Null
		eul.QUA_Euler	=	New QUA_Euler
	EndIf

	;Generate the output rotation
	eul\x = -ATan2(sinv, cosv) ;  inverted due to change in handedness of coordinate system
	eul\y = ATan2(sint, cost)
	eul\z = ATan2(sinf, cosf)
	
End Function



; result will be the same rotation as doing q1 then q2 (order matters!)
Function QUA_MultiplyQuat#(Aw#,Ax#,Ay#,Az#,Bw#,Bx#,By#,Bz#)
	Local a#, b#, c#, d#, e#, f#, g#, h#
	a = (Aw + Ax) * (Bw + Bx)
	b = (Az - Ay) * (By - Bz)
	c = (Aw - Ax) * (By + Bz)
	d = (Ay + Az) * (Bw - Bx)
	e = (Ax + Az) * (Bx + By)
	f = (Ax - Az) * (Bx - By)
	g = (Aw + Ay) * (Bw - Bz)
	h = (Aw - Ay) * (Bw + Bz)
	q.QUA_Quat=Last QUA_Quat
	If q=Null q=New QUA_Quat
	q\w# = b + (-e - f + g + h) / 2
	q\x# = a - ( e + f + g + h) / 2
	q\y# = c + ( e - f + g - h) / 2
	q\z# = d + ( e - f - g + h) / 2
	Return q\w#
End Function


;===================================================================================
; ========== PRIVATE ===================================================
;===================================================================================

Function QUA_QuatToMatrix(q.QUA_Quat,m.QUA_matrix)

	Local a#, w#, x#, y#, z#;

	a = Sqr(q\w*q\w + q\x*q\x + q\y*q\y + q\z*q\z)

	If (a >= EPSILON) Then
		w = q\w *a
		x = q\x *a
		y = q\y *a
		z = q\z *a
		
		QUA_matrixIdentity(m)
		
		m\m00 = 1 - 2*(y*y + z*z)
		m\m01 = 2*(x*y + w*z)
		m\m02 = 2*(x*z - w*y)

		m\m10 = 2*(x*y - w*z)
		m\m11 = 1 - 2*(x*x + z*z)
		m\m12 = 2*(y*z + w*x)

		m\m20 = 2*(x*z + w*y)
		m\m21 = 2*(y*z - w*x)
		m\m22 = 1 - 2*(x*x + y*y)

	Else
		QUA_matrixIdentity(m)
	EndIf

End Function 



Function QUA_matrixIdentity(m.QUA_matrix)
	m\m00 = 1 
	m\m01 = 0
	m\m02 = 0
	m\m03 = 0

	m\m10 = 0
	m\m11 = 1
	m\m12 = 0
	m\m13 = 0

	m\m20 = 0
	m\m21 = 0
	m\m22 = 1
	m\m23 = 0

	m\m30 = 0
	m\m31 = 0
	m\m32 = 0
	m\m33 = 1
	Return True
End Function

I have to use it to get the rotation of B3D files
=> getting w,x,y,z from the parse of a b3d, and then i build the scene using createpivot and createmesh command.
All works fine, but I can't turn entitys like it should.

Bobysait

(Posted 2006) [#2]

Ok??? I think, the mistake is in the QUA_EulerToQuat function...

I replaced with


Function QUA_EulerToQuat2(Pitch#, Yaw#, Roll#)
	
	Pitch# = Pitch# / 2.0
	Yaw#   = Yaw#   / 2.0
	Roll#  = Roll#  / 2.0
	
	Cos_Pitch# = Cos(Pitch#)
	Cos_Yaw#   = Cos(Yaw#)
	Cos_Roll#  = Cos(Roll#);
	
	Sin_Pitch# = Sin(Pitch#)
	Sin_Yaw#   = Sin(Yaw#)
	Sin_Roll#  = Sin(Roll#);
	
	CpCy# = Cos_Pitch# * Cos_Yaw#
	SpSy# = Sin_Pitch# * Sin_Yaw#
	SpCy# = Sin_Pitch# * Cos_Yaw#
	CpSy# = Cos_Pitch# * Sin_Yaw#

	q.QUA_Quat=Last QUA_Quat
	If q=Null q=New QUA_Quat

	q\z = ((Sin_Roll# * CpCy#) - (Cos_Roll# * SpSy#))*-1
	q\x = ((Cos_Roll# * SpCy#) + (Sin_Roll# * CpSy#))*-1
	q\y = (Cos_Roll# * CpSy#) - (Sin_Roll# * SpCy#)
	q\w = (Cos_Roll# * CpCy#) + (Sin_Roll# * SpSy#)

End Function

and it seems to walk right

... Of course, it does not explain why 3 codes for getting Euler from Quat return 3 different values

Danny

(Posted 2006) [#3]

Sorry can't give you a direct answer to this, but I do know there's several versions and variations of Quaternion function libraries in the code archives! These libraries all handle the same basic functions (Euler to Quats, Quats to Eulers, Slerps, etc) so I think it's safe to say that you should be able to find a solution in one of there. Either copy one of their functions or fix/adjust your own using theirs to compare...

Hope that helps,
Danny.

Bobysait

(Posted 2006) [#4]

Thank you.

I'm just wondering if the exporter B3d pipeline store good quaternions values...
=> noone of the values contained in the chunk node seem to return the same values as using any one of the quaternions lib.
Would it be right to say, b3d pipeline lib don't store good values ? or i have to use an external lib to transform values into valid ones ?

b32	(Posted 2006) [#5]

I've used Leadwerks conversion:

;-----------------------------------------------------------------------------------------------------
; 										    RotationToString$()
;-----------------------------------------------------------------------------------------------------
;convert rotation information to string
Function RotationToString$(ptime, iPitch#, iYaw#, iRoll#)	

	;this code was written by LeadWerks
	;http://www.blitzbasic.com/Community/posts.php?topic=51579
	sp# = Sin(iYaw   / 2)
	cp# = Cos(iYaw   / 2)
	sy# = Sin(iRoll  / 2)
	cy# = Cos(iRoll  / 2)
	sr# = Sin(iPitch / 2)
	cr# = Cos(iPitch / 2)
	
	w# = + (cr * cp * cy - sr * sp * sy)
	x# = - (sr * cp * cy - cr * sp * sy)
	y# = + (cr * sp * cy + sr * cp * sy)
	z# = - (sr * sp * cy + cr * cp * sy)
	
	Return ptime + "; 4; " + w + "," + x + "," + y + "," + z + ";;;"
	
End Function

It works very nice. However, I found another problem:

; CreateCube Example
; ------------------

Graphics3D 640,480,0,2
SetBuffer BackBuffer()

camera=CreateCamera()
PositionEntity camera, 0, 0, -10

; Create cube
cube=CreateCube()
TurnEntity cube, 0, 0, 270, 1

While Not KeyDown( 1 )

	RenderWorld
	
		Text 0, 0, EntityRoll(cube, 1)

	Flip
Wend

End

Even though I turn the entity 270 degrees, the outcome is -90. The result looks the same, but for saving animations, this means the cube will rotate in the wrong direction. It will allways take the shortest route. Not sure if this helps, but I thought I post it anyway.

Bobysait

(Posted 2006) [#6]

I understand the problem it occurs. and maybe that's why quaternions i use return Euler Angles >360 or <360 and maybe it's better.

Thanks for your code, i've tried it, and the same problem happen.
One way : I looked on the milkshape exporter for b3d, and maybe the problem is that blitz3d axis are not logical.

As we can see

QUA_eulerToQuat(X#,Y#,Z#)
	rz#=x#
	rx#=y#
	ry#=z#
	r#=rz
	p#=rx#
	y#=ry#
	sp#=Sin(p/2):cp=Cos(p/2)
	sy#=Sin(y/2):cy=Cos(y/2)
	sr#=Sin(r/2):cr=Cos(r/2)
	quat[0]=cr*cp*cy + sr*sp*sy;
	quat[1]=sr*cp*cy - cr*sp*sy;
	quat[2]=cr*sp*cy + sr*cp*sy;
	quat[3]=cr*cp*sy - sr*sp*cy;

	quat[3]=-quat[3];

Mark need to convert axis to get those of blitz3d.
Maybe quaternions libraries don't rectify this. ( not sure )
but as, i get quaternions form b3d file, they have to be in the good axis... I don't know how to explain all that, but i think rotations of b3d files are not as simple as "turnEntity Rx,Ry,Rz" using local coordinates from conversion of quaternions to Euler included in the b3d file.
I found the code from Pacemaker for import/export b3d files, but it does not attempt to build a scene using the b3d files.
It seems that noone has ever try it before ? maybe, I try to do something stupid :D

b32	(Posted 2006) [#7]

After reading a few topics, I understand that Blitz quaternions are different from normal quaternions. I can't seem to find out what the difference is exactly. A while ago, I was writing a X-exporter. I noticed at one point that I was doing the order of xyzw/wxyz wrong somehow. Maybe it is something similair?

Bobysait

(Posted 2006) [#8]

Maybe should Mark explain it...

Bobysait

(Posted 2006) [#9]

after looking MiniB3D library, i found a way !

rotations have to be converted like this :
QuatToEUler(-w,x,y,-z)
rx-pitch
ry=+yaw
rz=+roll