AlignToVector

BlitzMax Forums/MiniB3D Module/AlignToVector

simonh

(Posted 2006) [#1]

Right, rather than derail the 'XEdition' thread I'll start a new topic. The aim - to create a working AlignToVector function.

Here's my attempt:

	Function AlignToVector(vx#,vy#,vz#,axis,rate#=1.0)
	
		Local pitch#=Abs(EntityPitch())
		Local yaw#=Abs(EntityYaw())
		Local roll#=Abs(EntityRoll())

		Select axis
		
			Case 1
			
				Local dest_yaw#=Abs(ATan2(vz#,vx#))
				Local dest_roll#=Abs(ATan2(vy#,vx#))
			
				yaw#=UpdateValue#(yaw#,dest_yaw#,rate#)
				roll#=UpdateValue#(roll#,dest_roll#,rate#)

			Case 2
				
				Local dest_pitch#=Abs(ATan2(vz#,vy#))
				Local dest_roll#=Abs(-ATan2(vx#,vy#))
			
				pitch#=UpdateValue#(pitch#,dest_pitch#,rate#)
				roll#=UpdateValue#(roll#,dest_roll#,rate#)

			Case 3
			
				Local dest_pitch#=Abs(-ATan2(vy#,vz#))
				Local dest_yaw#=Abs(-ATan2(vx#,vz#))
	
				pitch#=UpdateValue#(pitch#,dest_pitch#,rate#)
				yaw#=UpdateValue#(yaw#,dest_yaw#,rate#)

		End Select
		
		Self.RotateEntity pitch#,yaw#,roll#
	
	End Function
	
	Function UpdateValue#(current#,destination#,rate#)
	
		'rate#=1.0/rate#
	
		current#=current#+((destination#-current#)*rate#)
	
		Return current#
	
	End Function

And example code (BMX):

Import "../MiniB3D.bmx"

Graphics3D 640,480

' set this to 1, 2 Or 3
axis=1

' set vector
vx#=0.0
vy#=1.0
vz#=0.0

camera=CreateCamera()
PositionEntity camera,0,2,-5
RotateEntity camera,15,0,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.2,0.2,0.2
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

While Not KeyDown(KEY_ESCAPE)
	
	AlignToVector cone,vx,vy,vz,axis,0.01

	RenderWorld
	
	glColor3f 255,0,0
	Text 0,0,"X"
	glColor3f 0,255,0
	Text 0,20,"Y"
	glColor3f 0,0,255
	Text 0,40,"Z"
	
	Flip
	
Wend

End

Function Text(x,y,text$)

	' reset texture matrix
	glMatrixMode(GL_TEXTURE)		
	glLoadIdentity()
		
	' set active texture to texture 0 so gldrawtext will work correctly
	glActiveTextureARB(GL_TEXTURE0)
	glClientActiveTextureARB(GL_TEXTURE0)
	
	' enable texture 2D
	glEnable(GL_TEXTURE_2D)
	
	glDisable(GL_LIGHTING)

	GLDrawText text$,x,y
	
	glEnable(GL_LIGHTING)
	
	' disable texture 2D - needed as gldrawtext enables it, but doesn't disable after use
	glDisable(GL_TEXTURE_2D)
	
End Function

Example code (B3D):

Graphics3D 640,480

; 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

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.2,0.2,0.2
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

While Not KeyDown(1)
	
	AlignToVector cone,vx,vy,vz,axis,0.01

	RenderWorld
	
	Color 255,0,0
	Text 0,0,"X"
	Color 0,255,0
	Text 0,20,"Y"
	Color 0,0,255
	Text 0,40,"Z"
	
	Flip
	
Wend

End

The bmx version must work the same as the b3d version. The above version works some of the time, but some vector inputs won't work - such as x=0,y=-1,0,z=0.0,axis=2.

Also, don't forget that this command can be used twice or three times to combine the axis rotations.

Amon	(Posted 2006) [#2]

Ahhh! Thanks very much Simon. I appreciate you helping us. I can't test it myself but I'm sure antony will pop in to have a look at it.

Again, Thank You very much for your help. :)

AntonyWells

(Posted 2006) [#3]

Good start, and thanks for helping out on the cause. We'll have a bump-mapped 3d engine in max oneday.

Not sure why yours doesn't work all the time, but if it works some of the time it must be on the right track.

Any chance mark will let just you sneak a peak at his aligntovector code to write us a version? No worries if not, just seems like the ideal solution.

FlameDuck

(Posted 2007) [#4]

The aim - to create a working AlignToVector function.

Isn't AlignToVector simply a SLERP? Thus, doesn't it stand to reason thst your version doesn't work, because it's using Euler angle, and thus prone to gimbal lockage?

Floyd

(Posted 2007) [#5]

When the 'rate' parameter was added to AlignToVector I was curious about what it really meant.
Some experimenting suggested it was doing this:

1. The axis to be aligned and the target vector determine a plane.
2. The entity turns about an axis perpendicular to this plane.
3. The rate gives the fraction of the "angle difference" to turn.

This last part means that a rate of 0.5 will turn half the way to the target.

Graphics3D 500, 400, 0, 2
SetBuffer FrontBuffer()

piv = CreatePivot() : TurnEntity piv, 0, 160, 0

For n = 1 To 5
	Print EntityYaw( piv )
	AlignToVector piv, 0, 0, 1,    3, 0.5   ; turn half the way to the target
Next

WaitKey : End

One possible glitch arises if the original axis and the target vector point in exactly opposite directions. In that case they do not determine a plane and the problem become ill-defined. Try changing 160 to 180 in the example.

JoshK

(Posted 2007) [#6]

I just did a small test with some terrain I am working on. The Y axis worked when I got rid of the Abs().

For smooth angular interpolation, you have to use quaternions and perform a slerp routine.

-=TinBlue=-

(Posted 2007) [#7]

Perhaps I'm being thick but the code submitted at the start of the thread doesn't work.

Compile Error: To Many Function Parameters

Matty

(Posted 2009) [#8]

Was align to vector ever added to minib3d?
It's a very useful function...I used it all the time in blitz3d? Great for AI units in space combat sims.

Warner

(Posted 2009) [#9]

I have written an AlignToVector function for my mod, but I'm not sure if it will work with the standard minib3d version.

'-----------------------------------------------------------------------------------------------------------------------------------
'											AlignToVector2()
'-----------------------------------------------------------------------------------------------------------------------------------
Function AlignToVector2(e:TEntity, x#, y#, z#, axis=3, rate#=1.0)

	If axis<1 Or axis>3 Then Return

	Local ax#,ay#,az#
	Local dd#
	Local m:TMatrix = TMatrix.Create()
	
	'normalize
	dd# = Magnitude(x,y,z)
	If dd < 0.000001 Then Return
	x = x / dd
	y = y / dd
	z = z / dd	

	'get original axis	
	ax# = (axis=1)
	ay# = (axis=2)
	az# = (axis=3)
	TFormNormal ax, ay, az, e, Null
	ax =  TFormedX()
	ay =  TFormedY()
	az =  TFormedZ()
	
 	'get transformation matrix from org. axis to new one
	m.FromToRotation(ax,ay,-az, x,y,-z)
	
	'interpolate
	If rate < 1.0 Then
		m:TMatrix = InterpolateMatrix(m, rate)
	End If
	
     'apply matrix
	e.rotmat.Multiply2(m)
'	e.SetUpdateFlag
'	e.MQ_Update()
			 		
End Function

TMatrix.bmx:

'Version 12 june 12:17
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()
	
		grid[0,0]=1.0 grid[1,0]=0.0 grid[2,0]=0.0 grid[3,0]=0.0
		grid[0,1]=0.0 grid[1,1]=1.0 grid[2,1]=0.0 grid[3,1]=0.0
		grid[0,2]=0.0 grid[1,2]=0.0 grid[2,2]=1.0 grid[3,2]=0.0		
		grid[0,3]=0.0 grid[1,3]=0.0 grid[2,3]=0.0 grid[3,3]=1.0
	
	End Method
	
	' copy - create new copy and returns it
	
	Method Copy:TMatrix()
	
		Local mat:TMatrix=New TMatrix
	
		mat.grid[0,0]=grid[0,0]
		mat.grid[1,0]=grid[1,0]
		mat.grid[2,0]=grid[2,0]
		mat.grid[3,0]=grid[3,0]
		mat.grid[0,1]=grid[0,1]
		mat.grid[1,1]=grid[1,1]
		mat.grid[2,1]=grid[2,1]
		mat.grid[3,1]=grid[3,1]
		mat.grid[0,2]=grid[0,2]
		mat.grid[1,2]=grid[1,2]
		mat.grid[2,2]=grid[2,2]
		mat.grid[3,2]=grid[3,2]
		
		' do not remove
		mat.grid[0,3]=grid[0,3]
		mat.grid[1,3]=grid[1,3]
		mat.grid[2,3]=grid[2,3]
		mat.grid[3,3]=grid[3,3]
		
		Return mat
	
	End Method
	
	' overwrite - overwrites self with matrix passed as parameter
	
	Method Overwrite(mat:TMatrix)
	
		grid[0,0]=mat.grid[0,0]
		grid[1,0]=mat.grid[1,0]
		grid[2,0]=mat.grid[2,0]
		grid[3,0]=mat.grid[3,0]
		grid[0,1]=mat.grid[0,1]
		grid[1,1]=mat.grid[1,1]
		grid[2,1]=mat.grid[2,1]
		grid[3,1]=mat.grid[3,1]
		grid[0,2]=mat.grid[0,2]
		grid[1,2]=mat.grid[1,2]
		grid[2,2]=mat.grid[2,2]
		grid[3,2]=mat.grid[3,2]		
		grid[0,3]=mat.grid[0,3]
		grid[1,3]=mat.grid[1,3]
		grid[2,3]=mat.grid[2,3]
		grid[3,3]=mat.grid[3,3]
		
	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#)
	
		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
		
		' 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()
	
	 Local x:Int,y:Int
	
	 Local a:Double[,] = New Double[4,4]
	
	 For x = 0 To 3
	 For y = 0 To 3
		 a[y,x] = grid[x,y]
	 Next
	 Next
	
	 For x = 0 To 3
	 For y = 0 To 3
		grid[x,y]=a[x,y]
	 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, j
	
	  '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 = 0 To 2
	      For j = 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
     If (q1_w > 1) Then		
		dd# = (q1_x*q1_x + q1_y*q1_y + q1_z*q1_z + q1_w*q1_w)
		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 - q1_w * q1_w)

     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

Romanski

(Posted 2009) [#10]

something is strange... is there any fix for this?

Robert Cummings

(Posted 2010) [#11]

bump :)

Looking for a simple AlignToVector (2) that I can port to iminib3d too otherwise I can't get it running on pc/mac as well as the ipad...

If its simple maths I can port to C++ no problem but the above uses a different TMatrix...

Warner

(Posted 2010) [#12]

That's somewhat difficult, because I believe you need matrices to do that right. Here is what I tried:

Function AlignToVector(e:TEntity, x#, y#, z#)

	'yaw-pitch-roll
	
	Local yaw#,pitch#,roll#
	Local x1#,y1#,z1#
	Local x2#,y2#,z2#
	Local x3#,y3#,z3#
	
	yaw# = -ATan2(x, z)
	
	x1# = z*Sin(yaw) + x*Cos(yaw)
	y1# = y	
	z1# = z*Cos(yaw) - x*Sin(yaw)
	
	pitch# = -ATan2(y1, z1)	
	x2# = x1
	y2# = y1*Cos(pitch) - z1*Sin(pitch)
	z2# = y1*Sin(pitch) + z1*Cos(pitch)
	
	roll# = -ATan2(x2, y2)
	'x3# = x2*Cos(roll) - y2*Sin(roll)
	'y3# = x2*Sin(roll) + y2*Cos(roll)
	'z3# = z2	
		
	RotateEntity e, pitch, yaw, roll
	
End Function

Demo:

Strict

Import sidesign.minib3d

Graphics3D 800, 600, 0, 2

CreateLight()

Local cube:TMesh = CreateCone()
RotateMesh cube, 90, 0, 0

Local cam:TCamera = CreateCamera()
MoveEntity cam, 0, 5, -15
PointEntity cam, cube

Local p# = 0

Repeat
	
	p :+ 1
	AlignToVector(cube, Cos(p), Sin(p), Cos(p*3))
	
	RenderWorld
		
	Flip
	
Until KeyHit(27)

End

Function AlignToVector(e:TEntity, x#, y#, z#)

	'order=yaw-pitch-roll
	
	Local yaw#,pitch#,roll#
	Local x1#,y1#,z1#
	Local x2#,y2#,z2#
	Local x3#,y3#,z3#
	
	yaw# = -ATan2(x, z)
	
	x1# = z*Sin(yaw) + x*Cos(yaw)
	y1# = y	
	z1# = z*Cos(yaw) - x*Sin(yaw)
	
	pitch# = -ATan2(y1, z1)	
	x2# = x1
	y2# = y1*Cos(pitch) - z1*Sin(pitch)
	z2# = y1*Sin(pitch) + z1*Cos(pitch)
	
	roll# = -ATan2(x2, y2)
	'x3# = x2*Cos(roll) - y2*Sin(roll)
	'y3# = x2*Sin(roll) + y2*Cos(roll)
	'z3# = z2	
		
	RotateEntity e, pitch, yaw, roll
	
End Function

_Skully

(Posted 2010) [#13]

what JoshK said

Robert Cummings

(Posted 2010) [#14]

Thanks for the attempt Warner! I tried your code for AlignToVector but it won't align to the normal unfortunately. I'm just not sure why either! I would like to try and get an object to stand on a small sphere like super mario galaxy.

Thank you for the demo though, I will keep trying?

Whats happening is the character just turns away, is it possible it is mixing up a couple of numbers?

Last edited 2010

ima747

(Posted 2010) [#15]

Define turns away? Is it always flipped along 1 axis, or does it change at some points? OpenGl uses some inverted numbers in it's matrixies which could potentially be a factor depending on how they are applied to the existing matrix for the object...

Warner

(Posted 2010) [#16]

[edit]I think I see what you mean. Here is a slightly modified version:

Strict

Import sidesign.minib3d

Graphics3D 800, 600, 0, 2

CreateLight()

'cone 
Local cube:TMesh = CreateCone()
'RotateMesh cube, 90, 0, 0
'PositionMesh cube, 0, 0, 1

Local surf:TSurface = GetSurface(cube, 1)
VertexColor surf, 0, 255, 0, 0
VertexColor surf, 1, 255, 0, 0
VertexColor surf, 2, 255, 0, 0

EntityFX cube, 2

'camera
Local cam:TCamera = CreateCamera()
MoveEntity cam, 0, 5, -15
PointEntity cam, cube

'sphere
Local sph:TMesh = CreateSphere()
ScaleMesh sph, 5, 5, 5
EntityColor sph, 0, 255, 0
EntityPickMode sph, 2

surf = GetSurface(sph, 1)
For Local i% = 0 To CountVertices(surf) - 1
	VertexColor surf, i, 0, Rand(128, 255), 0
Next

EntityFX sph, 2

Repeat

	'turn sphere using keys
	TurnEntity sph, 0, KeyDown(39)-KeyDown(37), KeyDown(40)-KeyDown(38)
	
	'click on sphere to place cone
	If MouseDown(1)
		CameraPick cam, MouseX(), MouseY()
		PositionEntity cube, PickedX(), PickedY(), PickedZ()
		
		AlignToVector cube, PickedNX(), PickedNY(), PickedNZ()
	End If
		
	RenderWorld
	
	'beginmax2d
	'Local s$ = f(PickedNX()) + "," + f(PickedNY()) + "," + f(PickedNZ())
	'DrawText s$, 0, 0
	'endmax2d
		
	Flip

'esc=exit
Until KeyHit(27)

End

'align to vector
Function AlignToVector(e:TEntity, x#, y#, z#)

	Local fac# = 1
	
	'order=yaw-pitch-roll
	
	Local yaw#,pitch#,roll#
	Local x1#,y1#,z1#
	Local x2#,y2#,z2#
	Local x3#,y3#,z3#
	
	yaw# = -ATan2(x, z)
	
	x1# = z*Sin(yaw) + x*Cos(yaw)
	y1# = y	
	z1# = z*Cos(yaw) - x*Sin(yaw)
	
	pitch# = -ATan2(y1, z1)
	x2# = x1
	y2# = y1*Cos(pitch) - z1*Sin(pitch)
	z2# = y1*Sin(pitch) + z1*Cos(pitch)
	
	roll# = -ATan2(x2, y2)
	x3# = x2*Cos(roll) - y2*Sin(roll)
	y3# = x2*Sin(roll) + y2*Cos(roll)
	z3# = z2

	'FIX
	If y < 0 roll :+ 180
	
	RotateEntity e, pitch, yaw, roll
	
End Function

'shortest distance between two angles
Function AngDiff:Float(a1:Float,a2:Float)
	While (a1 < 0) a1 :+ 360 Wend
	While (a2 < 0) a2 :+ 360 Wend
	a1 = a1 Mod 360
	a2 = a2 Mod 360
	
	Local d1# = (a2 - a1)
	Local d2# = (a2 + 360 - a1)
	Local d3# = (a2 - 360 - a1)
	
	If Abs(d2) < Abs(d1) d1 = d2
	If Abs(d3) < Abs(d1) d1 = d3
	
	Return d1
	
End Function

'swap two float variables
Function Swap(x# Var, y# Var)
	Local temp# = x
	x = y
	y = temp
End Function

'format float string
Function f$(x#)

	x# = Floor(x * 1000) / 1000.0
	Local t$ = x
	If Instr(t$, ".") > 0 Then t$ = Left$(t$, Instr(t$, ".") + 4)
	Return t$

End Function

Last edited 2010

Robert Cummings

(Posted 2010) [#17]

Wow that looks like it does the trick! I don't know how to thank you :)

Warner

(Posted 2010) [#18]

No problem, I do hope it suffices for what you need it for. The 'fix' line starts with if 'y < 0'. Could turn out that should be 'if y<=0' instead.

Last edited 2010