Intense math question

Blitz3D Forums/Blitz3D Programming/Intense math question

Chroma

(Posted 2007) [#1]

Basically I'm stuck here on orientation using a quaternion. I start by defining a starting euler of 0,0,0. Then I convert that into a quat.

Then I take a thrust vector of 0,1,0 (so the thrust will go straight up from the aircraft local space). Now I multiply that thrust vector by the amount of thrust (say 100) which gives me a thrust vector of 0,100,0. Now I take this thrust vector and multiply it by the orientation quat. The object moves straight up...good. Now when I turn this quat on pitch and roll axis everything is still good. If I pitch forward the object moves forward and same with roll. The bad thing is when I yaw, the object continues to act like there was no yaw input and while even though the object is turned 90 degrees left, if i pitch it forward, it still moves forward in worldspace but sideways. For some reason the yawing of the quat isn't turning the thrust vector on the y-axis.

Here's some pseudo code:
vForces = (0,0,0)
vThrust = (0,100,0) ;apply 100 force straight up
vForces = vForces + vThrust ;add thrust to main force
qOrientation = qOrientation * vForcest
vEuler = QuatToEuler( qOrientation )
RotateEntity blah, vEuler\x, vEuler\y, vEuler\z

It's pretty straight forward but somewhere the yaw is getting ignored.

Something else I did that was pretty cool. I took the TFormNormal of the (0,1,0) and did a local to global space convert and used the TFormedXYZ as forces and it worked great...until I gave yaw input...then it went haywire.

So the main question is, how do you make a defined vector stick to the orientation of an object? I want my thrust vector to always be straight up from the object (local space) no matter what the orientation of the object is.

Stevie G

(Posted 2007) [#2]

This kind of thing will always happen with rotateentity as it seems to processes the rotations in a certain order.

While pitch and roll work fine together , yaw always goes nuts. Probably gimbal related. How about using a separate pivot for yaw so that it yaws in local rather than global space - like so ...

Piv = createpivot()
Blah = loadmesh( "MyMesh" , Piv )
...
Rotateentity Piv, v\Euler\x , 0 , v\Euler\z
Rotateentity Blah, 0 , v\Euler\y, 0

Chroma

(Posted 2007) [#3]

After some research I found the recommend rotation order is yaw, then pitch, then roll. I understand the basic gist of the quaternion representing the rotation and then you just multiply the force vector by the orientation and the result is the acceleration (a=f/m) that's added to velocity etc.

But it seems when I multiply the forces which in this case is 100 units of forces straight up (0,100,0) by the orientation quat it gets messed up.

I also found some ironclad euler to quat and quat to euler code:

Function EulerToQuat(q.Quat, yaw#, pitch#, roll#)
	Local c1# = Cos(yaw/2)
	Local c2# = Cos(pitch/2)
	Local c3# = Cos(roll/2)
	Local s1# = Sin(yaw/2)
	Local s2# = Sin(pitch/2)
	Local s3# = Sin(roll/2)
	
	Local c1c2# = c1*c2
	Local s1s2# = s1*s2
	
	q\w = c1c2*c3 - s1s2*s3
  	q\x = c1c2*s3 + s1s2*c3
	q\y = s1*c2*c3 + c1*s2*s3
	q\z = c1*s2*c3 - s1*c2*s3
End Function

; Quat can be non-normalized
Function QuatToEuler2(v.Vector, q.Quat)	;output: x=yaw, y=pitch, z=roll
	Local sqw# = q\w*q\w
	Local sqx# = q\x*q\x
	Local sqy# = q\y*q\y
	Local sqz# = q\z*q\z

	Local unit# = sqx + sqy + sqz + sqw; if normalised is one, otherwise is correction factor
	Local test# = q\x*q\y + q\z*q\w
	If test > 0.499 * unit	; singularity at north pole
		v\x = 2 * ATan2(q\x,q\w)
		v\y = Pi/2
		v\z = 0
		Return
	EndIf
	If test < -0.499 * unit ; singularity at south pole
		v\x = -2 * ATan2(q\x,q\w)
		v\y = -Pi/2
		v\z = 0
		Return
	EndIf
	v\x = ATan2(2*q\y*q\w-2*q\x*q\z , sqx - sqy - sqz + sqw)
	v\y = ASin(2*test/unit)
	v\z = ATan2(2*q\x*q\w-2*q\y*q\z , -sqx + sqy - sqz + sqw)
End Function

I know the above code works. I've monitored the input eulers and output eulers and they match up. The quat functions are from euclideanspace btw.

Chroma

(Posted 2007) [#4]

Taking a force vector of (0,100,0) and running it through this function should return the modified force vector based on the orientation of the quat...but it's not working. If I pitch or roll it acts right, but the yaw just isn't happening. Plus when you apply thrust straight up on the y-axis as long as you're straight everything is fine. But if you yaw the thrust goes down but it shouldn't. With just yawing around with no pitch or roll input, the thrust should remain constant...but it diminishes but comes back when the object is facing forward again. Still working on this...I need a beer.

EDIT: Could this be blitz3d specific problem? I don't see anyone else having this same Quat/Vector rotation problem anywhere else.

Function QuatMulVec(r.Vector, q.Quat, v.Vector)
	Local vectorQuat.Quat = New Quat
	Local inverseQuat.Quat = New Quat
	Local resultQuat.Quat = New Quat
	
	vectorQuat\w = 0.0
	vectorQuat\x = v\x
	vectorQuat\y = v\y
	vectorQuat\z = v\z

	QuatCopy(inverseQuat, q)
	QuatInverse(inverseQuat)
	
	QuatMul(resultQuat, q, vectorQuat)
	QuatMul(resultQuat, resultQuat, inverseQuat)

	r\x = resultQuat\x
	r\y = resultQuat\y
	r\z = resultQuat\z

	Delete vectorQuat
	Delete inverseQuat
	Delete resultQuat
End Function