Thanks, those things arn't giving me much trouble though, it's just a case of finding a working Turn function and something to tform a vector around a quat's rotation.
Here's the quat class I threw together from my own code, some of halo's and a smidgen of mark's, which in theory covers all my needs.
Only rotate works 100% though... Don't suppose you can see a problem with it?
Type Quat
Function create:quat()
Return New quat
End Function
Function quatToMat( this:quat )
q[0]=this.x
q[1]=this.y
q[2]=this.z
q[3]=this.w
Local xx#=q[1]*q[1]
Local yy#=q[2]*q[2]
Local zz#=q[3]*q[3];
Local xy#=q[1]*q[2]
Local xz#=q[1]*q[3]
Local yz#=q[2]*q[3];
Local wx#=q[0]*q[1]
Local wy#=q[0]*q[2]
Local wz#=q[0]*q[3];
mat[0]=1-2*(yy+zz);mat[1]= 2*(xy-wz);mat[2]= 2*(xz+wy);
mat[3]= 2*(xy+wz);mat[4]=1-2*(xx+zz);mat[5]= 2*(yz-wx);
mat[6]= 2*(xz-wy);mat[7]= 2*(yz+wx);mat[8]=1-2*(xx+yy);
End Function
Method Tform_Vector( x!,y!,z! )
quat.quatToMat( Self )
Local temp:vector = vector.create3d( x,y,z )
quat.matMulVec(temp)
tformx=quat.vecx
tformy=quat.vecy
tformz=quat.vecz
End Method
Field tformx!,tformy!,tformz!
Global q:Float[]
Global mat:Float[]
Global vecx!,vecy!,vecz!
Function matMulVec( vec:vector )
vecx=mat[0] * vec.vec[0] + mat[3] * vec.vec[1] + mat[6] * vec.vec[2];
vecy=mat[1] * vec.vec[0] + mat[4] * vec.vec[1] + mat[7] * vec.vec[2];
vecz=mat[2] * vec.vec[0] + mat[5] * vec.vec[1] + mat[8] * vec.vec[2];
End Function
Method SetEuler( npitch!,nyaw!,nroll! )
quat.EulerAsQuat( npitch,nyaw,nroll )
w=quat.vectorw
x=quat.vectorx
y=quat.vectory
z=quat.vectorz
updateReal()
End Method
Method UpdateReal()
quat.QuatAsEuler(x,y,z,w)
pitch=quat.vectorx
yaw=quat.vectory
roll=quat.vectorz
End Method
Field Pitch#,Yaw#,Roll#
Field w!,x!,y!,z!
Global vectorw#,vectorx#,vectory#,vectorz#
Method MultiQuat( With:Quat )
quat.mulQuat( x,y,z,w,with.x,with.y,with.z,with.w)
w=quat.vectorw
x=quat.vectorx
y=quat.vectory
z=quat.vectorz
End Method
Method Clone( Copy:Quat )
w=copy.w
x=copy.x
y=copy.y
z=copy.z
End Method
Method Turn( nPitch!,nYaw!,nRoll!)
Local Temp:Quat = quat.create()
temp.SetEuler(npitch,nyaw,nroll)
self.multiquat( temp )
End Method
Function EulerAsQuat(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#
vectorw#=cr#*cpcy#+sr#*spsy#
vectorx#=sr#*cpcy#-cr#*spsy#
vectory#=cr#*spcy#+sr#*cpsy#
vectorz#=cr#*cpsy#-sr#*spcy#
End Function
Function QuatAsEuler(x#,y#,z#,w#)
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
vectorz#=-ATan2(sinv,cosv)
vectorx#=ATan2(sint,cost)
vectory#=ATan2(sinf,cosf)
End Function
Function MulQuat(Ax#,Ay#,Az#,Aw#,Bx#,By#,Bz#,Bw#)
Local a#=(Aw#+Ax#)*(Bw#+Bx#)
Local b#=(Az#-Ay#)*(By#-Bz#)
Local c#=(Aw#-Ax#)*(By#+Bz#)
Local d#=(Ay#+Az#)*(Bw#-Bx#)
Local e#=(Ax#+Az#)*(Bx#+By#)
Local f#=(Ax#-Az#)*(Bx#-By#)
Local g#=(Aw#+Ay#)*(Bw#-Bz#)
Local h#=(Aw#-Ay#)*(Bw#+Bz#)
vectorw#=b#+(-e#-f#+g#+h#)/2.0
vectorx#=a#-(e#+f#+g#+h#)/2.0
vectory#=c#+(e#-f#+g#-h#)/2.0
vectorz#=d#+(e#-f#-g#+h#)/2.0
End Function
End Type
quat.q = New Float[4]
quat.mat = New Float[15]
|
