Help with Quat manipulation....

BlitzMax Forums/BlitzMax Programming/Help with Quat manipulation....

The r0nin (Posted 2008) [#1]

I have the starting point for a path (say it is x=0, y=50, and z=30). I have an Euler converted to a Quat for the original facing of a curved path (call it "dir" and it was 0,0,90 in pitch,yaw,and roll) and a Euler converted to a Quat for the final facing of the path (the "bend" which is 0,-30,0). What I need to know how to calculate is the final coordinates (x1,y1,z1) of the path if the path is 15 units in length. As far as I can tell, I multiply dir by bend to get the Quat describing the final rotation. How do I then use the resulting Quat mathematically to get the final position (x1,y1,z1) 15 units from the original position? Any help from the experts would be greatly appreciated...

The r0nin (Posted 2008) [#2]

OK... I found out that I need to normalize the Quats after multiplying them. What's next... multiplying the resulting quat by the quat equivalent of 15,0,0?

Damien Sturdy (Posted 2008) [#3]

I would think you convert the quat to a vector in some way, then multiply that by 15,15,15.

The r0nin (Posted 2008) [#4]

Here's what I have so far... I have two different methods of rotating the objects (moddir and mod2dir) but neither matches up with the actual angling of the object and it's movement. The quat and vec3 code is from Leadwerk's code. I'm stumped... Strict Import sidesign.minib3d Local width:Int=800,height:Int=600,depth:Int=32,mode:Int=1 Local move:Float =2.0 Local ps:Int = 150 Local ht:Int = 100 Local pathlen:Float = Sqr((14.3*14.3)+(4.1*4.1)) Local pathang:Float = ASin(4.1/14.3) Graphics3D width,height,depth,mode Local cam:TCamera = CreateCamera() PositionEntity cam,0,ht,0 cam.CameraRange(1,5000) Local cam2:Tcamera = CreateCamera() PositionEntity Cam2,0,ht,0 cam2.CameraViewport 0,400,200,200 cam2.RotateEntity 90,0,0 cam2.EntityParent(cam) Local light:TLight = CreateLight(1) LightRange(light,5000) PositionEntity light,0,700,0 '***********************MAP***************** Local terr1:TMesh = LoadMesh("resource/map/map1.b3d") Local terr2:TMesh = LoadMesh("resource/map/map2.b3d") Local terr3:TMesh = LoadMesh("resource/map/map3.b3d") Local terr4:TMesh = LoadMesh("resource/map/map4.b3d") Local tex:TTexture = LoadTexture("resource/map/grass6.bmp") PositionEntity terr1,0,0,0 ScaleEntity terr1,ps,ht,ps EntityTexture terr1,tex PositionEntity terr2,0,0,ps*4 ScaleEntity terr2,ps,ht,ps EntityTexture terr2,tex PositionEntity terr3,0,0,-ps*4 ScaleEntity terr3,ps,ht,ps EntityTexture terr3,tex PositionEntity terr4,ps*4,0,0 ScaleEntity terr4,ps,ht,ps EntityTexture terr4,tex '*********************Sky********************* Local sky:TMesh=CreateSphere(12) ScaleEntity sky,1000,500,1000 FlipMesh sky EntityFX sky,1 Local sky_tex:TTexture=LoadTexture("resource/textures/skybox.png") EntityTexture sky,sky_tex light.EntityParent(sky) '*********************Fighter******************** Local player:Tfighter = Tfighter.Make(-10,50,30) Local shadow:Tfighter = Tfighter.Make(10,50,30) EntityTexture (shadow.air,shadow.redtex) '*********************Setup ****************** Local cx#=0 Local cy#=0 Local cz#=0 Local pitch#=0 Local yaw#=0 Local roll#=0 ' used by fps code Local old_ms:Float=MilliSecs() Local renders:Float Local fps:Float '******************* Step loop ***************** HideMouse() '**********************Main Loop *************** Repeat If KeyHit(KEY_ENTER) Then DebugStop ' control camera If KeyDown(KEY_UP) Then cz#=cz#+move If KeyDown(KEY_LEFT) Then cx#=cx#-move If KeyDown(KEY_RIGHT) Then cx#=cx#+move If KeyDown(KEY_DOWN) Then cz#=cz#-move If KeyDown(KEY_W) Then pitch#=pitch#+1.0 If KeyDown(KEY_A) Then yaw#=yaw#+1.0 If KeyDown(KEY_S) Then pitch#=pitch#-1.0 If KeyDown(KEY_D) Then yaw#=yaw#-1.0 If KeyHit(KEY_H) Then player.Move() shadow.Move() End If If KeyHit(KEY_J) Then player.ModDir(0,30,0) shadow.Mod2Dir(0,30,0) End If If KeyHit(KEY_G) Then player.ModDir(0,-30,0) shadow.Mod2Dir(0,-30,0) End If If KeyHit(KEY_Y) Then player.ModDir(30,0,0) shadow.Mod2Dir(30,0,0) End If If KeyHit(KEY_N) Then player.ModDir(-30,0,0) shadow.Mod2Dir(-30,0,0) End If If KeyHit(KEY_I) Then player.ModDir(0,0,30) shadow.Mod2Dir(0,0,30) End If If KeyHit(KEY_K) Then player.ModDir(0,0,-30) shadow.Mod2Dir(0,0,-30) End If MoveEntity cam,cx#*0.5,cy#*0.5,cz#*0.5 PositionEntity(sky,EntityX(cam),0,EntityZ(cam)) RotateEntity cam,pitch#,yaw#,roll# cx#=0 cy#=0 cz#=0 cx#=0 cy#=0 cz#=0 RenderWorld renders=renders+1 ' calculate fps If MilliSecs()-old_ms>=1000 old_ms=MilliSecs() fps=renders renders=0 EndIf BeginMax2D() DrawText "FPS: "+String(fps),0,0 DrawText "Memory: "+GCMemAlloced(),0,20 DrawText "X: "+player.x+", Y: "+player.y+", Z: "+player.z,0,40 DrawText "Phi: "+player.phi+", Theta: "+player.theta+", Psi: "+player.psi,0,60 EndMax2D() Flip 1 Until KeyDown(KEY_ESCAPE) Or AppTerminate() End Type Tfighter Field x#,y#,z#,oldx#,oldy#,oldz# Field air:TMesh = LoadMesh ("resource/models/MECVair2.b3d") Field airtex:TTexture = LoadTexture ("resource/textures/MECVair2.png") Field redtex:TTexture = LoadTexture ("resource/textures/pathbase.png") Field phi%,theta%, psi%,oldphi%,oldtheta%,oldpsi% Field thrust%=15,lift#=0.0 Function Make:Tfighter (xin#=0.0,yin#=0.0,zin#=0.0,pitch%=0,yaw%=0,roll%=0) Local temp:Tfighter = New Tfighter temp.x = xin temp.y = yin temp.z = zin temp.phi = -pitch temp.theta = -yaw temp.psi = -roll temp.thrust=15 EntityTexture (temp.air,temp.airtex) PositionEntity (temp.air, temp.x,temp.y,temp.z) RotateEntity(temp.air,temp.phi,temp.theta,temp.psi) Return temp End Function Method Move() oldx = x oldy = y oldz = z Local speed% = 0 x=x+(Cos(theta)*Cos(phi)*thrust)'+((Sin(theta)*Sin(phi)-Cos(theta)*Sin(phi)*Cos(psi))*lift) z=z+(Sin(theta)*Cos(phi)*thrust)'+((-Cos(theta)*Sin(psi)-Sin(theta)*Sin(phi)*Cos(psi))*lift) y=y+(Sin(phi)*thrust)'+(Cos(phi)*Cos(psi)*lift) PositionEntity(air,x,y,z) End Method Method SetDir(a%=0,b%=0,c%=0) oldphi=phi oldtheta=theta oldpsi=psi phi = a theta = b psi = c RotateEntity(air,phi,theta,psi) End Method Method ModDir(pitch%=0,yaw%=0,roll%=0) oldphi=phi oldtheta=theta oldpsi=psi phi = phi+pitch theta = theta-yaw psi = psi-roll RotateEntity(air,psi,theta,phi) End Method Method Mod2Dir(pitch%=0,yaw%=0,roll%=0) oldphi=phi oldtheta=theta oldpsi=psi Local tempeu:TVec3 = Vec3(phi,theta,psi) Local qdir:TQuat = tempeu.ToQuat() qdir.normalize() Local temp2:TVec3 = Vec3(pitch,yaw,roll) Local qchange:TQuat = temp2.toQuat() qchange.normalize() Local FinalQ:TQuat = qdir.times(qchange) finalq.normalize() phi=finalq.pitch() theta=Finalq.yaw() psi=finalq.roll() RotateEntity(air,phi,theta,psi,True) End Method End Type Function Vec3:TVec3( x#=0.0,y#=0.0/0.0,z#=0.0/0.0 ) Local t:TVec3=New TVec3 If IsNan(y) y=x If IsNan(z) z=y t.x=x t.y=y t.z=z Return t EndFunction Function Quat:TQuat( x#,y#,z#,w# ) Local t:TQuat=New TQuat t.x=x t.y=y t.z=z t.w=w Return t EndFunction Type TVec3 {value} Field x#,y#,z# {attribute} Method Length#() Return Sqr( x*x+y*y+z*z ) End Method Method Dot#( v:TVec3 ) Return x*v.x+y*v.y+z*v.z End Method Method Inverse:TVec3() Return Vec3( -x,-y,-z ) End Method Method Reciprocal:TVec3() Return Vec3( 1/x,1/y,1/z ) End Method Method Normalize:TVec3() Local t#=Sqr( x*x+y*y+z*z ) If t=0.0 Return vec3(0.0,0.0,0.0) Return Vec3( x/t,y/t,z/t ) End Method Method Scale:TVec3( scale# ) Return Vec3( x*scale,y*scale,z*scale ) End Method Method DistanceTo#( v:TVec3 ) Local dx#=x-v.x,dy#=y-v.y,dz#=z-v.z Return Sqr( dx*dx+dy*dy+dz*dz ) End Method Method Plus:TVec3( v:TVec3 ) Return Vec3( x+v.x,y+v.y,z+v.z ) End Method Method Minus:TVec3( v:TVec3 ) Return Vec3( x-v.x,y-v.y,z-v.z ) End Method Method Times:TVec3( v:TVec3 ) Return Vec3( x*v.x,y*v.y,z*v.z ) End Method Method DividedBy:TVec3( v:TVec3 ) Return Vec3( x/v.x,y/v.y,z/v.z ) End Method Method Cross:TVec3( v:TVec3 ) Return Vec3( y*v.z-z*v.y,z*v.x-x*v.z,x*v.y-y*v.x ) End Method Method Blend:TVec3( v:TVec3,Alpha# ) Local beta#=1-Alpha Return Vec3( x*beta+v.x*Alpha,y*beta+v.y*Alpha,z*beta+v.z*Alpha ) End Method Method ToQuat:TQuat() Local c1#=Cos(-z/2) Local s1#=Sin(-z/2) Local c2#=Cos(-x/2) Local s2#=Sin(-x/2) Local c3#=Cos( y/2) Local s3#=Sin( y/2) Local c1_c2#=c1*c2 Local s1_s2#=s1*s2 Local t:TQuat=New TQuat t.x=c1*s2*c3 - s1*c2*s3; t.y=c1_c2*s3 + s1_s2*c3; t.z=s1*c2*c3 + c1*s2*s3; t.w=c1_c2*c3 - s1_s2*s3; Return t EndMethod Function FromQuat:TVec3(quat:TQuat) Return quat.toeuler() EndFunction Method Compare:Int(o:Object) Local t:TVec3=TVec3(o) If x<t.x Return -1 If y<t.y Return -1 If z<t.z Return -1 If x>t.x Return 1 If y>t.y Return 1 If z>t.z Return 1 Return 0 EndMethod EndType Type TQuat Field x#,y#,z#,w#=1.0 {attribute} Method Copy:TQuat() Return Quat( x,y,z,w ) End Method Method ToString$() Return "Quat("+x+","+y+","+z+","+w+")" End Method Method Inverse:TQuat() Return Quat( -x,-y,-z,w ) End Method Method ToAngleAxis( angle# Var,axis:TVec3 Var ) Local t#=ACos(w) angle=t*2 If angle>0 t=1/Sin(t) axis=Vec3( x*t,y*t,z*t ) Else axis=Vec3(0,0,0) EndIf End Method Method Yaw#() Return ATan2( 2*y*w-2*z*x,1-2*y*y-2*x*x ) End Method Method Pitch#() Return -ASin( 2*z*y+2*x*w ) End Method Method Roll#() Return -ATan2( 2*z*w-2*y*x,1-2*z*z-2*x*x ) End Method Method ToEuler:TVec3() Local x2#=x*x,y2#=y*y,z2#=z*z Local euler:TVec3=New TVec3 euler.y#=ATan2( 2*y*w-2*z*x,1-2*y2-2*x2 ) euler.x#=-ASin( 2*z*y+2*x*w ) euler.z#=-ATan2( 2*z*w-2*y*x,1-2*z2-2*x2 ) Return euler End Method Method Times:TQuat( q:TQuat ) Local result:TQuat=New TQuat result.x#=w*q.x + x*q.w + y*q.z - z*q.y result.y#=w*q.y + y*q.w + z*q.x - x*q.z result.z#=w*q.z + z*q.w + x*q.y - y*q.x result.w#=w*q.w - x*q.x - y*q.y - z*q.z Return result EndMethod Method Slerp:TQuat( q:TQuat,a# ) Local b#=1-a,f Local d#=x*q.x+y*q.y+z*q.z+w*q.w If d<0 d=-d f=True EndIf If d<1 Local om#=ACos( d )'- Local si#=Sin( om )'- a=Sin( a*om )/si'- b=Sin( b*om )/si'- EndIf If f a=-a Return Quat( x*b + q.x*a , y*b + q.y*a , z*b + q.z*a , w*b + q.w*a ) EndMethod Method Normalize:TQuat() Local q:TQuat=New TQuat Local m#=Sqr(x*x+y*y+z*z+w*w) q.x=x/m q.y=y/m q.z=z/m q.w=w/m Return q EndMethod Function FromAngleAxis:TQuat( angle#,axis:TVec3 ) angle:/2 Local s#=Sin(angle) Local t:TQuat=New TQuat t.x=s*axis.x t.y=s*axis.y t.z=s*axis.z t.w=Cos(angle) Return t EndFunction Function FromEuler:TQuat(euler:TVec3) Return euler.toquat() EndFunction EndType