Implementing Quaternions

BlitzMax Forums/MiniB3D Module/Implementing Quaternions

JoshK

(Posted 2007) [#1]

I am going to be working on adding quaternions rotation today. I am also working with the modelview matrix scaled 0,0,-1 after the camera is oriented, because this eliminates the need for flipped positions, and gets rid of a lot of wackiness.

I'll be on MSN under ___________, if anyone with a good knowledge of this is available. I have done quat -> mat and back before, but it is still going to be tricky.

I'll post whatever I come up with, so you can use it with minib3d or whatever.

JoshK

(Posted 2007) [#2]

Okay, my quat code worked right off the bat. Run this in Blitz3D and you will see it produces identical results:

Graphics3D 640,480,32,2

Dim mat#(3,3)


SeedRnd MilliSecs()

e=CreatePivot()

x#=Rnd(-180,180)
y#=Rnd(-180,180)
z#=Rnd(-180,180)

RotateEntity e,x,y,z
RotateMatrix x,y,z

displaymatrix e

WaitKey
End

Function DisplayMatrix(e)
	Print "Blitz3D:"
	Print GetMatElement(e,0,0)+", "+GetMatElement(e,0,1)+", "+GetMatElement(e,0,2)
	Print GetMatElement(e,1,0)+", "+GetMatElement(e,1,1)+", "+GetMatElement(e,1,2)
	Print GetMatElement(e,2,0)+", "+GetMatElement(e,2,1)+", "+GetMatElement(e,2,2)
	Print GetMatElement(e,3,0)+", "+GetMatElement(e,3,1)+", "+GetMatElement(e,3,2)
	Print ""
	Print "My matrix:"
	Print mat(0,0)+", "+mat(0,1)+", "+mat(0,2)
	Print mat(1,0)+", "+mat(1,1)+", "+mat(1,2)
	Print mat(2,0)+", "+mat(2,1)+", "+mat(2,2)
	Print mat(3,0)+", "+mat(3,1)+", "+mat(3,2)
End Function

Function RotateMatrix(x#,y#,z#)
	EulerAsQuat x,y,z
	RotateMatrix4 VECTOR_X,VECTOR_Y,VECTOR_Z,VECTOR_W
End Function

Function PositionMatrix(x#,y#,z#)
	mat(3,0)=x
	mat(3,1)=y
	mat(3,2)=z
End Function

Function RotateMatrix4(x#,y#,z#,w#)
	mat(0,0)=(1.0-2.0*x*x-2.0*z*z)
	mat(1,0)=-(2.0*z*y-2.0*w*x)
	mat(2,0)=-(2.0*x*y+2.0*w*z)
	mat(0,1)=-(2.0*z*y+2.0*w*x)
	mat(1,1)=(1.0-2.0*y*y-2.0*x*x)
	mat(2,1)=-(2.0*w*y-2.0*x*z)
	mat(0,2)=2.0*w*z-2.0*x*y
	mat(1,2)=2.0*x*z+2.0*w*y
	mat(2,2)=1.0-2.0*z*z-2.0*y*y
End Function

Global VECTOR_X#
Global VECTOR_Y#
Global VECTOR_Z#
Global VECTOR_W#

Function EulerAsQuat(pitch#,yaw#,roll#)
	cr#=Cos(-roll#/2.0)
	cp#=Cos(pitch#/2.0)
	cy#=Cos(yaw#/2.0)
	sr#=Sin(-roll#/2.0)
	sp#=Sin(pitch#/2.0)
	sy#=Sin(yaw#/2.0)
	cpcy#=cp#*cy#
	spsy#=sp#*sy#
	spcy#=sp#*cy#
	cpsy#=cp#*sy#
	VECTOR_w#=cr#*cpcy#+sr#*spsy#
	VECTOR_x#=sr#*cpcy#-cr#*spsy#
	VECTOR_y#=cr#*spcy#+sr#*cpsy#
	VECTOR_z#=cr#*cpsy#-sr#*spcy#
End Function

Now added additional matrix rotation methods based on quaternions:

	'Uses Quaternion instead of Eulers
	Method TForm4(px#,py#,pz#,qx#,qy#,qz#,w#,sx#,sy#,sz#)
	
		' identity
		grid[0,3]=0.0
		grid[1,3]=0.0
		grid[2,3]=0.0
		grid[3,3]=1.0
	
		' translate
		grid[3,0] = px#
		grid[3,1] = py#
		grid[3,2] = pz#
	
		' rotate + scale
		grid[0,0]=(1.0-2.0*qx*qx-2.0*qz*qz)*sx
		grid[1,0]=-(2.0*qz*qy-2.0*w*qx)*sx
		grid[2,0]=-(2.0*qx*qy+2.0*w*qz)*sx
		
		grid[0,1]=-(2.0*qz*qy+2.0*w*qx)*sy
		grid[1,1]=(1.0-2.0*qy*qy-2.0*qx*qx)*sy
		grid[2,1]=-(2.0*w*qy-2.0*qx*qz)*sy
		
		grid[0,2]=(2.0*w*qz-2.0*qx*qy)*sz
		grid[1,2]=(2.0*qx*qz+2.0*w*qy)*sz
		grid[2,2]=(1.0-2.0*qz*qz-2.0*qy*qy)*sz
	End Method

	
	Method Rotate4(qx#,qy#,qz#,w#)
		
		grid[0,0]=(1.0-2.0*qx*qx-2.0*qz*qz)
		grid[1,0]=-(2.0*qz*qy-2.0*w*qx)
		grid[2,0]=-(2.0*qx*qy+2.0*w*qz)
		
		grid[0,1]=-(2.0*qz*qy+2.0*w*qx)
		grid[1,1]=(1.0-2.0*qy*qy-2.0*qx*qx)
		grid[2,1]=-(2.0*w*qy-2.0*qx*qz)
		
		grid[0,2]=2.0*w*qz-2.0*qx*qy
		grid[1,2]=2.0*qx*qz+2.0*w*qy
		grid[2,2]=1.0-2.0*qz*qz-2.0*qy*qy
		
	EndMethod

JoshK

(Posted 2007) [#3]

Okay, it seems to work when you find every instance of -rx or -z and don't flip them. The only thing I am not sure how to do is retrieve the global quaternion.

I've got it pretty close here, which shows how to extract a quat from a mat, but the signs get flipped randomly:

Global VECTOR_X#
Global VECTOR_Y#
Global VECTOR_Z#
Global VECTOR_W#

Local grid#[3,3]

SeedRnd MilliSecs()

x#=Rnd(-180,180)
y#=Rnd(-180,180)
z#=Rnd(-180,180)

eulerasquat x,y,z

qx#=VECTOR_x
qy#=VECTOR_Y
qz#=VECTOR_Z
qw#=VECTOR_W

grid[0,0]=(1.0-2.0*qx*qx-2.0*qz*qz)
grid[1,0]=-(2.0*qz*qy-2.0*qw*qx)
grid[2,0]=-(2.0*qx*qy+2.0*qw*qz)

grid[0,1]=-(2.0*qz*qy+2.0*qw*qx)
grid[1,1]=(1.0-2.0*qy*qy-2.0*qx*qx)
grid[2,1]=-(2.0*qw*qy-2.0*qx*qz)

grid[0,2]=2.0*qw*qz-2.0*qx*qy
grid[1,2]=2.0*qx*qz+2.0*qw*qy
grid[2,2]=1.0-2.0*qz*qz-2.0*qy*qy

VECTOR_Y#=Sqr(grid[0,0]-grid[1,1]-grid[2,2]+1.0)/2.0
VECTOR_Z#=Sqr(-grid[0,0]+grid[1,1]-grid[2,2]+1.0)/2.0
VECTOR_X#=Sqr(-grid[0,0]-grid[1,1]+grid[2,2]+1.0)/2.0
VECTOR_W#=Sqr(grid[0,0]+grid[1,1]+grid[2,2]+1.0)/2.0

Print qx+", "+qy+", "+qz+", "+qw
Print VECTOR_x+", "+VECTOR_y+", "+VECTOR_z+", "+VECTOR_w



Function EulerAsQuat(pitch#,yaw#,roll#)
	cr#=Cos(-roll#/2.0)
	cp#=Cos(pitch#/2.0)
	cy#=Cos(yaw#/2.0)
	sr#=Sin(-roll#/2.0)
	sp#=Sin(pitch#/2.0)
	sy#=Sin(yaw#/2.0)
	cpcy#=cp#*cy#
	spsy#=sp#*sy#
	spcy#=sp#*cy#
	cpsy#=cp#*sy#
	VECTOR_w#=cr#*cpcy#+sr#*spsy#
	VECTOR_x#=sr#*cpcy#-cr#*spsy#
	VECTOR_y#=cr#*spcy#+sr#*cpsy#
	VECTOR_z#=cr#*cpsy#-sr#*spcy#
End Function

JoshK

(Posted 2007) [#4]

Well, I have been trying all day, and I can't get this to work right. Without proper rotation, animation is impossible.

DJWoodgate

(Posted 2007) [#5]

Would it be worth looking at Blit3d's Geometry code that Mark posted as a sticky in the Blitz3d programming forum?

JoshK

(Posted 2007) [#6]

DAMN I finally got it working.

I parented one mesh to another and randomly spun the parent around on the screen. When I click the mouse, the child mesh gets parented to either the center mesh or the world. When I change the parenting, the mesh stays perfectly in place, no matter what crazy angle is happening. Here, see for yourself:
http://www.leadwerks.com/post/quat.zip

It will take some time to clean this up, but the hard part is done. I'm going to go drink now.

This extracts a quaternion from a matrix:

	Method ExtractQuat()
		fourWSquaredMinus1:Float = +grid[0,0]+grid[1,1]+grid[2,2]
		fourXSquaredMinus1:Float = +grid[0,0]-grid[1,1]-grid[2,2]
		fourYSquaredMinus1:Float = +grid[1,1]-grid[0,0]-grid[2,2]
		fourZSquaredMinus1:Float = +grid[2,2]-grid[0,0]-grid[1,1]

		biggestIndex=0
		fourBiggestSquaredMinus1:Float=fourWSquaredMinus1
		If fourXSquaredMinus1 > fourBiggestSquaredMinus1
			fourBiggestSquaredMinus1 = fourXSquaredMinus1 
			biggestIndex=1
		EndIf
		If fourYSquaredMinus1 > fourBiggestSquaredMinus1
			fourBiggestSquaredMinus1 = fourYSquaredMinus1 
			biggestIndex=2
		EndIf
		If fourZSquaredMinus1 > fourBiggestSquaredMinus1
			fourBiggestSquaredMinus1 = fourZSquaredMinus1 
			biggestIndex=3
		EndIf

		biggestValue:Float=Sqr(fourBiggestSquaredMinus1+1.0)*0.5
		mult:Float=0.25/biggestValue

		Select BiggestIndex
			Case 0
				w#=BiggestValue
				x#=(grid[1,2]-grid[2,1])*mult
				y#=-(grid[2,0]-grid[0,2])*mult
				z#=-(grid[0,1]-grid[1,0])*mult
			Case 1
				x#=-BiggestValue
				w#=-(grid[1,2]-grid[2,1])*mult
				y#=(grid[0,1]+grid(1,0))*mult
				z#=(grid[2,0]+grid[0,2])*mult
			Case 2
				y#=BiggestValue
				w#=-(grid[2,0]-grid[0,2])*mult
				x#=-(grid[0,1]+grid(1,0))*mult
				z#=(grid[1,2]+grid[2,1])*mult
			Case 3
				z#=-BiggestValue
				w#=(grid[0,1]-grid(1,0))*mult
				x#=(grid[2,0]+grid[0,2])*mult
				y#=-(grid[1,2]+grid[2,1])*mult
		EndSelect

		If w<-0.0
			x=-x
			y=-y
			z=-z
			w=-w
		EndIf

		VECTOR_X=z
		VECTOR_Y=x
		VECTOR_Z=y
		VECTOR_W=w		
	EndMethod

Wayne

(Posted 2007) [#7]

Damn fine work, I'm fixing a drink in your honor.

JoshK

(Posted 2007) [#8]

Scale is very hard to work into this. I might just let that go, since the only reason you would need it is if you are writing a mesh editor.

If one of the main programmers of this wants to email me, I will give you as much code and info as I can. I don't really want to put together a patch, because there are a lot of extra entities and things in MiniB3D I don't want to mess with.

One of the main changes I made was flipping the Z-axis, so positions and rotations can be stored with their true values, and the original triangle order is right. However, a lot of code was written on top of a faulty foundation, and it all needs to be fixed to work right. You vertex positions and normals will be reversed now, because they are being rendered the same as in Blitz3D.

Anyone know how to extract the scale from a rotated matrix? I thought I could just get the magnitude of each row, but that doesn't account for rotation.

simonh

(Posted 2007) [#9]

Personally I wouldn't bother struggling with this. I'm not sure it's worth the effort - all you're gaining is a slightly more robust EntityParent and the potential for a better TurnEntity (if implemented).

Otherwise, there are no benefits, as MinB3D works the same as Blitz3D in all other areas as far as I'm aware. Also, the aformentioned issues will be fixed at some point.

Bobysait

(Posted 2007) [#10]

I think it's a great idea using quaternions !
It permits lots of things like easy PointEntity, AlignToVector or Slerp functions...
It also reduces operators from 45 for matrix rotations to 28 for quaternions

Other :
Try to make a TurnEntity local using matrix... using quaternions, it's easy as qf=q0*qd ( where qf is the final rotation, q0 is the initial one, and qd is the delta rotation )

I think MiniB3d has implemented matrix, but no support for quat, and that's a big problem. you can't optimise calculs anymore, and no ability to have better entity system.
That's the reason why I finally attempt to do my own engine+wrapper.

What i don't understand is :
@SimonH
=> you released the bmax maths library, translated from mark's code... why don't you use it instead of matrix bases ?

Leon Drake

(Posted 2007) [#11]

does it still work using fitmesh?

*	(Posted 2007) [#12]

strange the turnentity function I posted on the forum works with what im using it for and still allows all the commands to work perfectly (Even my testers have tested it fine).