Quarternion code rewritten.

Blitz3D Forums/Blitz3D Programming/Quarternion code rewritten.

JoshK

(Posted 2003) [#1]

All I did was change the functions a little to work with my VectorX(),Y,Z conventions, which I find a little more robust. Since these return four values, the w value is returned by the function itself. This will be rewritten as a userlib, eventually, to go with the vector3D and plane3D libs.

; Quat.bb : v1.0 : 15/11/02
; A tutorial on how to use this file is at http://www.dscho.co.uk/blitz/tutorials/quaternions.shtml
Global vectorx#,vectory#,vectorz#

Function VectorX#()
Return vectorx#
End Function

Function VectorY#()
Return vectory#
End Function

Function VectorZ#()
Return vectorz#
End Function

; Change these constants if you notice slips in accuracy
Const QuatToEulerAccuracy# = 0.001
Const QuatSlerpAccuracy#   = 0.0001

;convert a Rotation to a Quat
Function EulerToQuat(pitch#,yaw#,roll#)
;NB roll is inverted due to change in handedness of coordinate systems
Local cr# = Cos(-roll/2)
Local cp# = Cos(pitch/2)
Local cy# = Cos(yaw/2)
Local sr# = Sin(-roll/2)
Local sp# = Sin(pitch/2)
Local sy# = Sin(yaw/2)
;These variables are only here to cut down on the number of multiplications
Local cpcy# = cp * cy
Local spsy# = sp * sy
Local spcy# = sp * cy
Local cpsy# = cp * sy
;Generate the output quat
w# = cr * cpcy + sr * spsy
vectorx = sr * cpcy - cr * spsy
vectory = cr * spcy + sr * cpsy
vectorz = cr * cpsy - sr * spcy
End Function

; convert a Quat to a Rotation
Function QuatToEuler(quatx#,quaty#,quatz#,quatw#)
Local sint#, cost#, sinv#, cosv#, sinf#, cosf#
Local cost_temp#
sint = (2 * quatw * quaty) - (2 * quatx * quatz)
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 * quaty * quatz) + (2 * quatw * quatx)) / cost
	cosv = (1 - (2 * quatx * quatx) - (2 * quaty * quaty)) / cost
	sinf = ((2 * quatx * quaty) + (2 * quatw * quatz)) / cost
	cosf = (1 - (2 * quaty * quaty) - (2 * quatz * quatz)) / cost
	Else
	sinv = (2 * quatw * quatx) - (2 * quaty * quatz)
	cosv = 1 - (2 * quatx * quatx) - (2 * quatz * quatz)
	sinf = 0
	cosf = 1
	EndIf
;Generate the output rotation
vectorx = -ATan2(sinv, cosv) ;  inverted due to change in handedness of coordinate system
vectory = ATan2(sint, cost)
vectorz = ATan2(sinf, cosf)
End Function

;use this to interpolate between quaternions
Function QuatSlerp(Ax#,Ay#,Az#,Aw#,Bx#,By#,Bz#,Bw#,t#)
Local scaler_w#, scaler_x#, scaler_y#, scaler_z#
Local omega#, cosom#, sinom#, scale0#, scale1#
cosom = Ax * Bx + Ay * By + Az * Bz + Aw * Bw
If cosom <= 0.0
	cosom = -cosom
	scaler_w = -Bw
	scaler_x = -Bx
	scaler_y = -By
	scaler_z = -Bz
	Else
	scaler_w = Bw
	scaler_x = Bx
	scaler_y = By
	scaler_z = Bz
	EndIf
If (1 - cosom) > QuatSlerpAccuracy
	omega = ACos(cosom)
	sinom = Sin(omega)
	scale0 = Sin((1 - t) * omega) / sinom
	scale1 = Sin(t * omega) / sinom
	Else
	;Angle too small: use linear interpolation instead
	scale0 = 1 - t
	scale1 = t
	EndIf
vectorx# = scale0 * start\x + scale1 * scaler_x
vectory# = scale0 * start\y + scale1 * scaler_y
vectorz# = scale0 * start\z + scale1 * scaler_z
w# = scale0 * start\w + scale1 * scaler_w
Return w#
End Function

; result will be the same rotation as doing q1 then q2 (order matters!)
Function MultiplyQuat#(Ax#,Ay#,Az#,Aw#,Bx#,By#,Bz#,Bw#)
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)
w# = b + (-e - f + g + h) / 2
vectorx# = a - ( e + f + g + h) / 2
vectory# = c + ( e - f + g - h) / 2
vectorz# = d + ( e - f - g + h) / 2
Return w#
End Function

Wavey

(Posted 2003) [#2]

Nice to see someone finding the code useful. I have to confess, though, I'm not sure why you rewrote it like this, but I'll assume you have your own conventions & reasons for using them.

My 2p worth: I think it may be confusing to users to handle quaternions in the same way you'd handle 3d vectors. True, a quaternion *is* a vector, but a normalized 4d vector, which this code was originally designed to hide from the user. Advanced users are welcome to inspect the {w, x, y, z} structure of the Quat, but I think it's useful to remind them that they are not normal 3d vectors, and shouldn't routinely be manipulated as such.

Also, I'd have problems returning (pitch, yaw, roll) information as a "vector" - I'd call it something else to avoid any confusion. It puts the responsibility on the caller to keep track of all their "vectors", whether they are 3d or 4d, and whether they're measured in units or degrees.

Having said that, I've not seen the rest of your code, and I don't doubt you have good reasons for doing it this way. In the end, it all comes down to individual taste & coding style.

JoshK

(Posted 2003) [#3]

I really hate unnecessary types. I like my commands to look as if they are internal Blitz commands, so I hate using vector\x# and that kind of thing. This can be handled more easily as a userlib.

Is there a way to transform a point with a quarternion?

Wavey

(Posted 2003) [#4]

Transforming a point: if you're talking about using matrices to manipulate points, I'm far from the best person to answer - have a look at http://www.gamasutra.com/features/19980703/quaternions_01.htm , which explains the maths behind quaternions in quite a friendly way (I also used this page extensively when writing the functions originally - many of the functions are basically exactly the same). The QuatToMatrix function there (listing 2) might be what you're after.