Attempt at AlignToVector

BlitzMax Forums/MiniB3D Module/Attempt at AlignToVector

SLotman (Posted 2010) [#1]

Here's my attempt - at finally implementing AlignToVector on miniB3D. For now, this routine doesn't implement the 'rate' parameter; can you guys test it out and see if it is working properly? Just changing vx,vy,vz and axis on the demo below it seems to work... Import sidesign.minib3d Graphics3D 640,480,0,2 '; set this To 1, 2 Or 3 axis=3 '; set vector vx#=0.0 vy#=1.0 vz#=0.0 camera=CreateCamera() PositionEntity camera,0,2,-5 RotateEntity camera,15,0,0 MoveEntity camera, 0, -1, 0 light=CreateLight() RotateEntity light,90,0,0 cone=CreateCone() xaxis=CreateCylinder(8,True,cone) RotateMesh xaxis,0,0,90 ScaleMesh xaxis,5.0,0.2,0.2 EntityColor xaxis,127,0,0 yaxis=CreateCylinder(8,True,cone) ScaleMesh yaxis,0.2,5.0,0.2 EntityColor yaxis,0,127,0 zaxis=CreateCylinder(8,True,cone) RotateMesh zaxis,90,0,0 ScaleMesh zaxis,0.2,0.2,5.0 EntityColor zaxis,0,0,127 marker=CreateSphere() ScaleEntity marker,0.3,0.3,0.3 PositionEntity marker,vx#,vy#,vz# '; change color of marker depending on axis selected Select axis Case 1 EntityColor marker,255,0,0 Case 2 EntityColor marker,0,255,0 Case 3 EntityColor marker,0,0,255 End Select Local minTimer:Long=MilliSecs() While Not KeyDown(KEY_ESCAPE) If MilliSecs()-minTimer>5000 Then vx=Rand(-1,1) vy=Rand(-1,1) vz=Rand(-1,1) If vx=0 And vy=0 And vz=0 Then vy=1 PositionEntity marker,vx#,vy#,vz# minTimer = MilliSecs() End If Align2Vector cone,vx,vy,vz,axis,1 RenderWorld beginmax2d() SetColor 255,0,0 DrawText "X:"+vx,0,0 SetColor 0,255,0 DrawText "Y:"+vy,0,20 SetColor 0,0,255 DrawText "Z:"+vz,0,40 endmax2d() Flip 1 Wend End Function Align2Vector(ent:TEntity, vx:Float,vy:Float,vz:Float,axis%, rate#=1) If axis<1 Or axis>3 Then Return Local ax#,ay#,az# Local dd# Local m:TMatrix = TMatrix.Create() RotateEntity ent,0,0,0 'normalize dd# = Magnitude(vx,vy,vz) If dd < 0.000001 Then Return vx = vx / dd vy = vy / dd vz = vz / dd 'get original axis ax# = (axis=1) ay# = (axis=2) az# = (axis=3) TFormNormal ax, ay, az, ent, Null ax = TFormedX() ay = TFormedY() az = TFormedZ() 'get transformation matrix from org. axis to new one m.FromToRotation(ax,ay,-az, vx,vy,-vz) Local ep#=ATan2( m.grid[2,1],Sqr( m.grid[2,0]*m.grid[2,0]+m.grid[2,2]*m.grid[2,2] ) ) If ep>360 Then ep:-360 Local a#=m.grid[2,0] Local b#=m.grid[2,2] If a#<=0.0001 And a#>=-0.0001 Then a#=0 If b#<=0.0001 And b#>=-0.0001 Then b#=0 Local ey#=ATan2(a#,b#) If ey>360 Then ey:-360 a#=m.grid[0,1] b#=m.grid[1,1] If a#<=0.0001 And a#>=-0.0001 Then a#=0 If b#<=0.0001 And b#>=-0.0001 Then b#=0 Local er#=ATan2(a#,b#) If er>360 Then ey:-360 TurnEntity ent, ep, ey, er, 1 End Function

GNS (Posted 2010) [#2]

What version of mb3d are you using? My version (slightly modified small fixes version) doesn't have a Create() method for TMatrix nor does it have the Magnitude() function.

SLotman (Posted 2010) [#3]

Ooops, I forgot I altered TMatrix too: basicly I mixed it up with some functions from Warner's "fix" (not fixing it actually, but just adding more stuff) and some optimizations from my part (you can see copymemory being used on some functions) Anyway, here's the code: 'Version 18 june 23:58 Type TMatrix Field grid#[4,4] Function Create:TMatrix() Local m:TMatrix = New TMatrix m.LoadIdentity() Return m End Function Method New() 'If LOG_NEW ' DebugLog "New TMatrix" 'EndIf End Method Method Delete() 'If LOG_DEL ' DebugLog "Del TMatrix" 'EndIf End Method Method LoadIdentity() MemClear (Varptr(grid[0,0]), SizeOf(grid)) grid[0,0]=1.0 grid[1,1]=1.0 grid[2,2]=1.0 grid[3,3]=1.0 End Method ' copy - create new copy and returns it Method Copy:TMatrix() Local mat:TMatrix=New TMatrix MemCopy( Varptr( mat.grid[0,0] ),Varptr( grid[0,0] ),SizeOf(grid) ) Return mat End Method ' overwrite - overwrites self with matrix passed as parameter Method Overwrite(mat:TMatrix) MemCopy( Varptr( grid[0,0] ),Varptr( mat.grid[0,0] ),SizeOf(grid) ) End Method Method Inverse:TMatrix() Local mat:TMatrix=New TMatrix Local tx#=0 Local ty#=0 Local tz#=0 ' The rotational part of the matrix is simply the transpose of the ' original matrix. mat.grid[0,0] = grid[0,0] mat.grid[1,0] = grid[0,1] mat.grid[2,0] = grid[0,2] mat.grid[0,1] = grid[1,0] mat.grid[1,1] = grid[1,1] mat.grid[2,1] = grid[1,2] mat.grid[0,2] = grid[2,0] mat.grid[1,2] = grid[2,1] mat.grid[2,2] = grid[2,2] ' The right column vector of the matrix should always be [ 0 0 0 1 ] ' in most cases. . . you don't need this column at all because it'll ' never be used in the program, but since this code is used with GL ' and it does consider this column, it is here. mat.grid[0,3] = 0 mat.grid[1,3] = 0 mat.grid[2,3] = 0 mat.grid[3,3] = 1 ' The translation components of the original matrix. tx = grid[3,0] ty = grid[3,1] tz = grid[3,2] ' Result = -(Tm * Rm) To get the translation part of the inverse mat.grid[3,0] = -( (grid[0,0] * tx) + (grid[0,1] * ty) + (grid[0,2] * tz) ) mat.grid[3,1] = -( (grid[1,0] * tx) + (grid[1,1] * ty) + (grid[1,2] * tz) ) mat.grid[3,2] = -( (grid[2,0] * tx) + (grid[2,1] * ty) + (grid[2,2] * tz) ) Return mat End Method Method Translate(x#,y#,z#) grid[3,0] = grid#[0,0]*x# + grid#[1,0]*y# + grid#[2,0]*z# + grid#[3,0] grid[3,1] = grid#[0,1]*x# + grid#[1,1]*y# + grid#[2,1]*z# + grid#[3,1] grid[3,2] = grid#[0,2]*x# + grid#[1,2]*y# + grid#[2,2]*z# + grid#[3,2] End Method Method Rotate(rx#,ry#,rz#) Local cos_ang#,sin_ang# ' yaw cos_ang#=Cos(ry#) sin_ang#=Sin(ry#) Local m00# = grid#[0,0]*cos_ang + grid#[2,0]*-sin_ang# Local m01# = grid#[0,1]*cos_ang + grid#[2,1]*-sin_ang# Local m02# = grid#[0,2]*cos_ang + grid#[2,2]*-sin_ang# grid[2,0] = grid#[0,0]*sin_ang# + grid#[2,0]*cos_ang grid[2,1] = grid#[0,1]*sin_ang# + grid#[2,1]*cos_ang grid[2,2] = grid#[0,2]*sin_ang# + grid#[2,2]*cos_ang grid[0,0]=m00# grid[0,1]=m01# grid[0,2]=m02# ' pitch cos_ang#=Cos(rx#) sin_ang#=Sin(rx#) ' *NEW* avoid creating those vars, reuse others already created! 'Local m10# = grid#[1,0]*cos_ang + grid#[2,0]*sin_ang 'Local m11# = grid#[1,1]*cos_ang + grid#[2,1]*sin_ang 'Local m12# = grid#[1,2]*cos_ang + grid#[2,2]*sin_ang m00# = grid#[1,0]*cos_ang + grid#[2,0]*sin_ang m01# = grid#[1,1]*cos_ang + grid#[2,1]*sin_ang m02# = grid#[1,2]*cos_ang + grid#[2,2]*sin_ang grid[2,0] = grid#[1,0]*-sin_ang + grid#[2,0]*cos_ang grid[2,1] = grid#[1,1]*-sin_ang + grid#[2,1]*cos_ang grid[2,2] = grid#[1,2]*-sin_ang + grid#[2,2]*cos_ang 'grid[1,0]=m10 'grid[1,1]=m11 'grid[1,2]=m12 grid[1,0]=m00 grid[1,1]=m01 grid[1,2]=m02 ' roll cos_ang#=Cos(rz#) sin_ang#=Sin(rz#) m00# = grid#[0,0]*cos_ang# + grid#[1,0]*sin_ang# m01# = grid#[0,1]*cos_ang# + grid#[1,1]*sin_ang# m02# = grid#[0,2]*cos_ang# + grid#[1,2]*sin_ang# grid[1,0] = grid#[0,0]*-sin_ang# + grid#[1,0]*cos_ang# grid[1,1] = grid#[0,1]*-sin_ang# + grid#[1,1]*cos_ang# grid[1,2] = grid#[0,2]*-sin_ang# + grid#[1,2]*cos_ang# grid[0,0]=m00# grid[0,1]=m01# grid[0,2]=m02# End Method Method RotatePitch(ang#) Local cos_ang#=Cos(ang#) Local sin_ang#=Sin(ang#) Local m10# = grid#[1,0]*cos_ang + grid#[2,0]*sin_ang Local m11# = grid#[1,1]*cos_ang + grid#[2,1]*sin_ang Local m12# = grid#[1,2]*cos_ang + grid#[2,2]*sin_ang grid[2,0] = grid#[1,0]*-sin_ang + grid#[2,0]*cos_ang grid[2,1] = grid#[1,1]*-sin_ang + grid#[2,1]*cos_ang grid[2,2] = grid#[1,2]*-sin_ang + grid#[2,2]*cos_ang grid[1,0]=m10 grid[1,1]=m11 grid[1,2]=m12 End Method Method RotateYaw(ang#) Local cos_ang#=Cos(ang#) Local sin_ang#=Sin(ang#) Local m00# = grid#[0,0]*cos_ang + grid#[2,0]*-sin_ang# Local m01# = grid#[0,1]*cos_ang + grid#[2,1]*-sin_ang# Local m02# = grid#[0,2]*cos_ang + grid#[2,2]*-sin_ang# grid[2,0] = grid#[0,0]*sin_ang# + grid#[2,0]*cos_ang grid[2,1] = grid#[0,1]*sin_ang# + grid#[2,1]*cos_ang grid[2,2] = grid#[0,2]*sin_ang# + grid#[2,2]*cos_ang grid[0,0]=m00# grid[0,1]=m01# grid[0,2]=m02# End Method Method RotateRoll(ang#) Local cos_ang#=Cos(ang#) Local sin_ang#=Sin(ang#) Local m00# = grid#[0,0]*cos_ang# + grid#[1,0]*sin_ang# Local m01# = grid#[0,1]*cos_ang# + grid#[1,1]*sin_ang# Local m02# = grid#[0,2]*cos_ang# + grid#[1,2]*sin_ang# grid[1,0] = grid#[0,0]*-sin_ang# + grid#[1,0]*cos_ang# grid[1,1] = grid#[0,1]*-sin_ang# + grid#[1,1]*cos_ang# grid[1,2] = grid#[0,2]*-sin_ang# + grid#[1,2]*cos_ang# grid[0,0]=m00# grid[0,1]=m01# grid[0,2]=m02# End Method Rem bbdoc: Returns a matrix made from the quaternion End Rem Method FromQuaternion(x#,y#,z#,w#) ' Based off of code in Axiom Local tx# = 2*x Local ty# = 2*y Local tz# = 2*z Local twx# = tx*w Local twy# = ty*w Local twz# = tz*w Local txx# = tx*x Local txy# = ty*x Local txz# = tz*x Local tyy# = ty*y Local tyz# = tz*y Local tzz# = tz*z grid[0,0] = 1.0-(tyy+tzz) grid[1,0] = txy-twz grid[2,0] = txz+twy grid[3,0] = 0 grid[0,1] = txy+twz grid[1,1] = 1.0-(txx+tzz) grid[2,1] = tyz-twx grid[3,1] = 0 grid[0,2] = txz-twy grid[1,2] = tyz+twx grid[2,2] = 1.0-(txx+tyy) grid[3,2] = 0 grid[0,3] = 0 grid[1,3] = 0 grid[2,3] = 0 grid[3,3] = 1 End Method Rem bbdoc: Transforms the matrix by another matrix End Rem Method Multiply(mat:TMatrix) Local m00# = grid#[0,0]*mat.grid#[0,0] + grid#[1,0]*mat.grid#[0,1] + grid#[2,0]*mat.grid#[0,2] + grid#[3,0]*mat.grid#[0,3] Local m01# = grid#[0,1]*mat.grid#[0,0] + grid#[1,1]*mat.grid#[0,1] + grid#[2,1]*mat.grid#[0,2] + grid#[3,1]*mat.grid#[0,3] Local m02# = grid#[0,2]*mat.grid#[0,0] + grid#[1,2]*mat.grid#[0,1] + grid#[2,2]*mat.grid#[0,2] + grid#[3,2]*mat.grid#[0,3] 'Local m03# = grid#[0,3]*mat.grid#[0,0] + grid#[1,3]*mat.grid#[0,1] + grid#[2,3]*mat.grid#[0,2] + grid#[3,3]*mat.grid#[0,3] Local m10# = grid#[0,0]*mat.grid#[1,0] + grid#[1,0]*mat.grid#[1,1] + grid#[2,0]*mat.grid#[1,2] + grid#[3,0]*mat.grid#[1,3] Local m11# = grid#[0,1]*mat.grid#[1,0] + grid#[1,1]*mat.grid#[1,1] + grid#[2,1]*mat.grid#[1,2] + grid#[3,1]*mat.grid#[1,3] Local m12# = grid#[0,2]*mat.grid#[1,0] + grid#[1,2]*mat.grid#[1,1] + grid#[2,2]*mat.grid#[1,2] + grid#[3,2]*mat.grid#[1,3] 'Local m13# = grid#[0,3]*mat.grid#[1,0] + grid#[1,3]*mat.grid#[1,1] + grid#[2,3]*mat.grid#[1,2] + grid#[3,3]*mat.grid#[1,3] Local m20# = grid#[0,0]*mat.grid#[2,0] + grid#[1,0]*mat.grid#[2,1] + grid#[2,0]*mat.grid#[2,2] + grid#[3,0]*mat.grid#[2,3] Local m21# = grid#[0,1]*mat.grid#[2,0] + grid#[1,1]*mat.grid#[2,1] + grid#[2,1]*mat.grid#[2,2] + grid#[3,1]*mat.grid#[2,3] Local m22# = grid#[0,2]*mat.grid#[2,0] + grid#[1,2]*mat.grid#[2,1] + grid#[2,2]*mat.grid#[2,2] + grid#[3,2]*mat.grid#[2,3] 'Local m23# = grid#[0,3]*mat.grid#[2,0] + grid#[1,3]*mat.grid#[2,1] + grid#[2,3]*mat.grid#[2,2] + grid#[3,3]*mat.grid#[2,3] Local m30# = grid#[0,0]*mat.grid#[3,0] + grid#[1,0]*mat.grid#[3,1] + grid#[2,0]*mat.grid#[3,2] + grid#[3,0]*mat.grid#[3,3] Local m31# = grid#[0,1]*mat.grid#[3,0] + grid#[1,1]*mat.grid#[3,1] + grid#[2,1]*mat.grid#[3,2] + grid#[3,1]*mat.grid#[3,3] Local m32# = grid#[0,2]*mat.grid#[3,0] + grid#[1,2]*mat.grid#[3,1] + grid#[2,2]*mat.grid#[3,2] + grid#[3,2]*mat.grid#[3,3] 'Local m33# = grid#[0,3]*mat.grid#[3,0] + grid#[1,3]*mat.grid#[3,1] + grid#[2,3]*mat.grid#[3,2] + grid#[3,3]*mat.grid#[3,3] grid[0,0]=m00 grid[0,1]=m01 grid[0,2]=m02 'grid[0,3]=m03 grid[1,0]=m10 grid[1,1]=m11 grid[1,2]=m12 'grid[1,3]=m13 grid[2,0]=m20 grid[2,1]=m21 grid[2,2]=m22 'grid[2,3]=m23 grid[3,0]=m30 grid[3,1]=m31 grid[3,2]=m32 'grid[3,3]=m33 End Method Rem bbdoc: Transforms a vector by the matrix End Rem Method TransformVec( rx# Var,ry# Var,rz# Var, addTranslation% = 0 ) Local w# = 1.0/ ( grid[0,3] + grid[1,3] + grid[2,3] + grid[3,3] ) Local ix# = rx# Local iy# = ry# Local iz# = -rz# addTranslation = Min( Max( addTranslation, 0 ), 1 ) rx = ( ( grid[0,0]*ix ) + ( grid[1,0]*iy ) + ( grid[2,0]*iz ) + grid[3,0] * addTranslation ) * w ry = ( ( grid[0,1]*ix ) + ( grid[1,1]*iy ) + ( grid[2,1]*iz ) + grid[3,1] * addTranslation ) * w rz = -( ( grid[0,2]*ix ) + ( grid[1,2]*iy ) + ( grid[2,2]*iz ) + grid[3,2] * addTranslation ) * w End Method Rem bbdoc: Transposes the matrix End Rem Method Transpose() For Local i% = 0 To 3 For Local j% = i + 1 To 3 Local save% = grid[i,j] grid[i,j] = grid[j,i] grid[j,i] = save Next Next End Method Rem bbdoc: Sets the translation elements of the matrix End Rem Method SetTranslate( x#, y#, z# ) grid[3,0] = x grid[3,1] = y grid[3,2] = z End Method Rem bbdoc: Scales the rotation elements of the matrix End Rem Method Scale(x#,y#,z#) grid[0,0] = grid#[0,0]*x# grid[0,1] = grid#[0,1]*x# grid[0,2] = grid#[0,2]*x# grid[1,0] = grid#[1,0]*y# grid[1,1] = grid#[1,1]*y# grid[1,2] = grid#[1,2]*y# grid[2,0] = grid#[2,0]*z# grid[2,1] = grid#[2,1]*z# grid[2,2] = grid#[2,2]*z# End Method Method Multiply2(i:TMatrix) Local m00# = grid[0,0] * i.grid[0,0] + grid[0,1] * i.grid[1,0] + grid[0,2] * i.grid[2,0] + grid[0,3] * i.grid[3,0] Local m01# = grid[0,0] * i.grid[0,1] + grid[0,1] * i.grid[1,1] + grid[0,2] * i.grid[2,1] + grid[0,3] * i.grid[3,1] Local m02# = grid[0,0] * i.grid[0,2] + grid[0,1] * i.grid[1,2] + grid[0,2] * i.grid[2,2] + grid[0,3] * i.grid[3,2] Local m03# = grid[0,0] * i.grid[0,3] + grid[0,1] * i.grid[1,3] + grid[0,2] * i.grid[2,3] + grid[0,3] * i.grid[3,3] Local m10# = grid[1,0] * i.grid[0,0] + grid[1,1] * i.grid[1,0] + grid[1,2] * i.grid[2,0] + grid[1,3] * i.grid[3,0] Local m11# = grid[1,0] * i.grid[0,1] + grid[1,1] * i.grid[1,1] + grid[1,2] * i.grid[2,1] + grid[1,3] * i.grid[3,1] Local m12# = grid[1,0] * i.grid[0,2] + grid[1,1] * i.grid[1,2] + grid[1,2] * i.grid[2,2] + grid[1,3] * i.grid[3,2] Local m13# = grid[1,0] * i.grid[0,3] + grid[1,1] * i.grid[1,3] + grid[1,2] * i.grid[2,3] + grid[1,3] * i.grid[3,3] Local m20# = grid[2,0] * i.grid[0,0] + grid[2,1] * i.grid[1,0] + grid[2,2] * i.grid[2,0] + grid[2,3] * i.grid[3,0] Local m21# = grid[2,0] * i.grid[0,1] + grid[2,1] * i.grid[1,1] + grid[2,2] * i.grid[2,1] + grid[2,3] * i.grid[3,1] Local m22# = grid[2,0] * i.grid[0,2] + grid[2,1] * i.grid[1,2] + grid[2,2] * i.grid[2,2] + grid[2,3] * i.grid[3,2] Local m23# = grid[2,0] * i.grid[0,3] + grid[2,1] * i.grid[1,3] + grid[2,2] * i.grid[2,3] + grid[2,3] * i.grid[3,3] Local m30# = grid[3,0] * i.grid[0,0] + grid[3,1] * i.grid[1,0] + grid[3,2] * i.grid[2,0] + grid[3,3] * i.grid[3,0] Local m31# = grid[3,0] * i.grid[0,1] + grid[3,1] * i.grid[1,1] + grid[3,2] * i.grid[2,1] + grid[3,3] * i.grid[3,1] Local m32# = grid[3,0] * i.grid[0,2] + grid[3,1] * i.grid[1,2] + grid[3,2] * i.grid[2,2] + grid[3,3] * i.grid[3,2] Local m33# = grid[3,0] * i.grid[0,3] + grid[3,1] * i.grid[1,3] + grid[3,2] * i.grid[2,3] + grid[3,3] * i.grid[3,3] grid[0,0]=m00 grid[0,1]=m01 grid[0,2]=m02 grid[0,3]=m03 grid[1,0]=m10 grid[1,1]=m11 grid[1,2]=m12 grid[1,3]=m13 grid[2,0]=m20 grid[2,1]=m21 grid[2,2]=m22 grid[2,3]=m23 grid[3,0]=m30 grid[3,1]=m31 grid[3,2]=m32 grid[3,3]=m33 End Method Rem bbdoc: Gets the current pitch of the matrix End Rem Method GetPitch#() Local x# = grid[2,0] Local y# = grid[2,1] Local z# = grid[2,2] Return -ATan2( y, Sqr( x*x+z*z ) ) End Method Rem bbdoc: Gets the current yaw of the matrix End Rem Method GetYaw#() Local x# = grid[2,0] Local z# = grid[2,2] Return ATan2( x,z ) End Method Rem bbdoc: Gets the current roll of the matrix End Rem Method GetRoll#() Local iy# = grid[0,1] Local jy# = grid[1,1] Return ATan2( iy, jy ) End Method '----------------------------------------------------------------------------------------------------------------------------------- ' FromToRotation() '----------------------------------------------------------------------------------------------------------------------------------- Rem * A Function For creating a rotation matrix that rotates a vector called * "from" into another vector called "to". * Input : from[3], To[3] which both must be *normalized* non-zero vectors * Output: mtx[3][3] -- a 3x3 matrix in colum-major form * Authors: Tomas Möller, John Hughes * "Efficiently Building a Matrix to Rotate One Vector to Another" * Journal of Graphics Tools, 4(4):1-4, 1999 EndRem Method FromToRotation:TMatrix(ix#,iy#,iz#, jx#,jy#,jz#) Local hvx#, hvz#, hvxy#, hvxz#, hvyz# Local dotu#, dotv#, dotuv# Local u#[3], v#[3] Local c1#,c2#,c3# Local x0#,x1#,x2# Local e#, h# Local i:Int, j:Int 'v = cross(from, To) v[0] = iy * jz - iz * jy v[1] = iz * jx - ix * jz v[2] = ix * jy - iy * jx 'e = DOT(from, To); e = ix * jx + iy * jy + iz * jz If (Abs(e) > 1.0 - 0.000001) Then '/* "from" And "to"-vector almost parallel */ x0# = Abs(ix) x1# = Abs(iy) x2# = Abs(iz) If (x0 < x1) If (x0 < x2) x0 = 1.0 x1 = 0.0 x2 = 0.0 Else x2 = 1.0 x0 = 0.0 x1 = 0.0 End If Else If (x1 < x2) x1 = 1.0 x0 = 0.0 x2 = 0.0 Else x2 = 1.0 x0 = 0.0 x1 = 0.0 End If End If u[0] = x0 - ix u[1] = x1 - iy u[2] = x2 - iz v[0] = x0 - jx v[1] = x1 - jy v[2] = x2 - jz dotu# = u[0] * u[0] + u[1] * u[1] + u[2] * u[2] dotv# = v[0] * v[0] + v[1] * v[1] + v[2] * v[2] dotuv# = u[0] * v[0] + u[1] * v[1] + u[2] * v[2] If dotu <> 0 Then c1# = 2.0 / dotu Else c1# = 10000 If dotv <> 0 Then c2# = 2.0 / dotv Else c2# = 10000 c3# = c1 * c2 * dotuv# For i:Int = 0 To 2 For j:Int = 0 To 2 grid[j,i] = - c1 * u[i] * u[j] .. - c2 * v[i] * v[j] .. + c3 * v[i] * u[j] Next grid[i,i] = grid[i,i] + 1.0; Next Else ' the most common Case, unless "from"="to", Or "from"=-"to" */ h = 1.0/(1.0 + e) '/* optimization by Gottfried Chen */ hvx = h * v[0] hvz = h * v[2] hvxy = hvx * v[1] hvxz = hvx * v[2] hvyz = hvz * v[1] grid[0,0] = e + hvx * v[0] grid[1,0] = hvxy - v[2] grid[2,0] = hvxz + v[1] grid[0,1] = hvxy + v[2] grid[1,1] = e + h * v[1] * v[1] grid[2,1] = hvyz - v[0] grid[0,2] = hvxz - v[1] grid[1,2] = hvyz + v[0] grid[2,2] = e + hvz * v[2] End If grid[3,0] = 0 grid[3,1] = 0 grid[3,2] = 0 grid[0,3] = 0 grid[1,3] = 0 grid[2,3] = 0 grid[3,3] = 1 End Method '----------------------------------------------------------------------------------------------------------------------------------- ' ToQuat() '----------------------------------------------------------------------------------------------------------------------------------- 'convert to quaternion Method ToQuat( qx# Var,qy# Var,qz# Var,qw# Var ) Local trace# = grid[0,0] + grid[1,1] + grid[2,2] + 1.0 Local s# If( trace > 0.0001 ) Then s = 0.5 / Sqr(trace) qw = 0.25 / s qx = ( grid[1,2] - grid[2,1] ) * s qy = ( grid[2,0] - grid[0,2] ) * s qz = ( grid[0,1] - grid[1,0] ) * s Else If ( grid[0,0] > grid[1,1]) And (grid[0,0] > grid[2,2]) Then s = 2.0 * Sqr( 1.0 + grid[0,0] - grid[1,1] - grid[2,2]) qw = (grid[1,2] - grid[2,1] ) / s qx = 0.25 * s qy = (grid[1,0] + grid[0,1] ) / s qz = (grid[2,0] + grid[0,2] ) / s ElseIf (grid[1,1] > grid[2,2]) Then s = 2.0 * Sqr( 1.0 + grid[1,1] - grid[0,0] - grid[2,2]) qw = (grid[2,0] - grid[0,2]) / s qx = (grid[1,0] + grid[0,1] ) / s qy = 0.25 * s qz = (grid[2,1] + grid[1,2] ) / s Else s = 2.0 * Sqr( 1.0 + grid[2,2] - grid[0,0] - grid[1,1] ) qw = (grid[0,1] - grid[1,0] ) / s qx = (grid[2,0] + grid[0,2] ) / s qy = (grid[2,1] + grid[1,2] ) / s qz = 0.25 * s End If End If End Method End Type Rem bbdoc: Gets the magnitude of the vector End Rem Function Magnitude#( x#, y#, z# ) Return Sqr( x*x + y*y + z*z ) End Function Rem bbdoc: Creates a quaternion from an angle and an axis End Rem Function Quaternion_FromAngleAxis( angle#, ax#,ay#,az#, rx# Var,ry# Var,rz# Var,rw# Var ) Local ha# = .5*angle Local sn# = Sin( ha ) rw# = Cos( ha ) rx# = sn * ax ry# = sn * ay rz# = sn * az End Function Rem bbdoc: Multiplies a quaternion End Rem Function Quaternion_MultiplyQuat( x1#,y1#,z1#,w1#, x2#,y2#,z2#,w2#, rx# Var,ry# Var,rz# Var,rw# Var ) rw = w1*w2 - x1*x2 - y1*y2 - z1*z2 rx = w1*x2 - x1*w2 - y1*z2 - z1*y2 ry = w1*y2 - y1*w2 - z1*x2 - x1*z2 rz = w1*z2 - z1*w2 - x1*y2 - y1*x2 End Function '----------------------------------------------------------------------------------------------------------------------------------- ' Interpolate() '----------------------------------------------------------------------------------------------------------------------------------- 'interpolates matrix Function InterpolateMatrix:TMatrix(a:TMatrix, alpha#) Local m:TMatrix = TMatrix.Create() Local q1_x#, q1_y#, q1_z#, q1_w# Local dd# a.ToQuat(q1_x, q1_y, q1_z, q1_w) If q1_w = 0 Then Return a.Copy() 'normalize Local q1q1w# = q1_w * q1_w If (q1_w > 1) Then dd# = (q1_x*q1_x + q1_y*q1_y + q1_z*q1_z + q1q1w) If dd <> 0 Then q1_x = q1_x / dd q1_y = q1_y / dd q1_z = q1_z / dd q1_w = q1_w / dd End If End If Local s# Local angle#,x#,y#,z# angle# = 2 * ACos(q1_w) s# = Sqr(1 - q1q1w) If (s < 0.001) Then x = q1_x y = q1_y z = q1_z Else x = q1_x / s y = q1_y / s z = q1_z / s End If angle = angle * alpha Quaternion_FromAngleAxis(angle, x,y,z, q1_x, q1_y, q1_z, q1_w) m.FromQuaternion(q1_x, q1_y, q1_z, q1_w) Return m End Function For testing reasons, you can just save it as "TMatrix-2.bmx" (as I have) and just change the include on minib3d.bmx - this way you get to keep the old one, and can actually test this one too :)

Robert Cummings (Posted 2010) [#4]

Is there a basic or C++ version of this as I would like to keep my minib3d pc and iphone versions compatible with each other. I have a similar issue with aligning an entity to some normals: http://www.blitzbasic.com/Community/posts.php?topic=92412 Any help or advice will be great!