Spherical travel and gimbal lock madness

Blitz3D Forums/Blitz3D Programming/Spherical travel and gimbal lock madness

Krischan

(Posted 2014) [#1]

I want to drive at constant speed on a sphere and without gimbal lock on the poles but I don't get it - it turns me mad. Imagine yourself in a vehicle driving over earth's surface and want to go from the USA to Russia - you wouldn't stop or turn around at the pole(s) or become faster there.

But how? I'm stuck and need some help. I turned 40 this year - I'm getting too old for such things?

Here is a small example, use the texture below as media "earth.jpg" and the cursor keys to drive around.

Graphics3D 1024, 768, 32, 2

; Media
Local tex = LoadTexture("earth.jpg")
Local map = LoadImage("earth.jpg")

ResizeImage(map, 400, 200)

; planet Center
Local pivot = CreatePivot()
PositionEntity pivot, 0, 0, 0

; planet
Local planet = CreateSphere(16, pivot)
PositionEntity planet, 0, 0, 0
EntityTexture planet, tex
EntityFX planet, 1

; Camera
Local cam = CreateCamera()
CameraRange cam, 0.1, 100
PositionEntity cam, 0, 0, -2

; player
Local player = CreateCube()
PositionEntity player, 0, 0, -1
EntityFX player, 1
EntityColor player, 255, 0, 0
ScaleEntity player, 0.01, 0.01, 0.01


While Not KeyHit(1)
	
	; cursor movement
	Local mx# = (KeyDown(203) - KeyDown(205)) * 0.01 * (1 + (KeyDown(54) * 2))
	Local my# = (KeyDown(200) - KeyDown(208)) * 0.01 * (1 + (KeyDown(54) * 2))
	
	; align player To planet
	MoveEntity player, mx, my, 0
	AlignPlayer(player, EntityX(player), EntityY(player), EntityZ(player))
	
	; align camera To player
	PositionEntity cam, EntityX(player, 1), EntityY(player, 1), EntityZ(player, 1)
	PointEntity cam, pivot
	MoveEntity cam, 0, 0, -1.5
	
	RenderWorld
	
	; calculate coordinates
	TFormPoint 0, 0, 0, cam, planet
	Local lat = ATan2(Sqr(TFormedX() ^ 2 + TFormedZ() ^ 2), + TFormedY()) - 90
	Local lon = ATan2(TFormedX(), -TFormedZ())
	If lat < - 90 Then lat = 90 Else If lat > 90 Then lat = 90
	If lon < - 180 Then lon = 180 Else If lon > 180 Then lon = 180
	
	; format coordinates
	Local lattext$, lontext$
	If lon >= 0 Then lontext = Int(Abs(lon)) + "E" Else lontext = Int(Abs(lon)) + "W"
	If lat <= 90 Then lattext = Int(Abs(lat)) + "N" Else lattext = Int(Abs(lat)) + "S"
	
	; minimap
	DrawImage map, 0, 0
	
	; draw player position
	Color 255, 0, 0
	Local x = Normalize(lon, -180, 180, 0, 400 - 1)
	Local y = Normalize(lat, -90, 90, 0, 200 - 1)
	Oval x - 2.5, y - 2.5, 5, 5
	
	; coordinates
	Color 255, 255, 255
	Text 0,200,"Position: " + lattext + "x" + lontext
	
	Flip
	
Wend

End

Function AlignPlayer(e, vx#, vy#, vz#)
	
	RotateEntity(e, VectorPitch(vx, vy, vz), VectorYaw(vx, vy, vz), EntityRoll(e))
	
End Function

Function Normalize#(value# = 128.0, value_min# = 0.0, value_max# = 255.0, norm_min# = 0.0, norm_max# = 1.0)
	
	Return ((value - value_min) / (value_max - value_min)) * (norm_max - norm_min) + norm_min
	
End Function

MiniB3D version:

SuperStrict

Framework sidesign.minib3d

Graphics3D 1024, 768, 32, 2

' Media
Local tex:TTexture = LoadTexture("earth.jpg")
Local map:TPixmap = LoadPixmap("earth.jpg")

map = ResizePixmap(map, 400, 200)

' Planet Center
Local pivot:TPivot = CreatePivot()
PositionEntity pivot, 0, 0, 0

' Planet
Local planet:TMesh = CreateSphere(16, pivot)
PositionEntity planet, 0, 0, 0
EntityTexture planet, tex
EntityFX planet, 1

' Camera
Local cam:TCamera = CreateCamera()
CameraRange cam, 0.1, 100
PositionEntity cam, 0, 0, -2

' Player
Local player:TMesh = CreateCube()
PositionEntity player, 0, 0, -1
EntityFX player, 1
EntityColor player, 255, 0, 0
ScaleEntity player, 0.01, 0.01, 0.01


While Not AppTerminate()

	If KeyHit(KEY_ESCAPE) Then End

	' cursor movement
	Local mx:Float = (KeyDown(KEY_LEFT) - KeyDown(KEY_RIGHT)) * 0.01 * (1 + (KeyDown(KEY_LSHIFT) * 2))
	Local my:Float = (KeyDown(KEY_UP) - KeyDown(KEY_DOWN)) * 0.01 * (1 + (KeyDown(KEY_LSHIFT) * 2))

	' align player to planet
	MoveEntity player, mx, my, 0
	AlignPlayer(player)

	' align camera to player
	PositionEntity cam, EntityX(player, 1), EntityY(player, 1), EntityZ(player, 1)
	PointEntity cam, pivot
	MoveEntity cam, 0, 0, -1.5
	
	RenderWorld
	
	BeginMax2D()

		' calculate coordinates
		TFormPoint 0, 0, 0, cam, planet
		Local lat:Float = ATan2(Sqr(TFormedX() ^ 2 + TFormedZ() ^ 2), + TFormedY()) - 90
		Local lon:Float = ATan2(TFormedX(), -TFormedZ())
		If lat < - 90 Then lat = 90 Else If lat > 90 Then lat = 90
		If lon < - 180 Then lon = 180 Else If lon > 180 Then lon = 180

		' format coordinates
		Local lattext:String, lontext:String
		If lon >= 0 Then lontext = Int(Abs(lon)) + "E" Else lontext = Int(Abs(lon)) + "W"
		If lat <= 90 Then lattext = Int(Abs(lat)) + "N" Else lattext = Int(Abs(lat)) + "S"
	
		' minimap
		DrawPixmap map, 0, 0

		' draw player position
		SetColor 255, 0, 0
		Local x:Float = Normalize(lon, -180, 180, 0, 400 - 1)
		Local y:Float = Normalize(lat, -90, 90, 0, 200 - 1)
		DrawOval x - 2.5, y - 2.5, 5, 5

		' coordinates
		SetColor 255, 255, 255
		DrawText "Position: " + lattext + "x" + lontext, 0, 200
						
	EndMax2D()
				
	Flip
	
Wend
	
End

Function AlignPlayer(e:TEntity)

	Local vx:Float = EntityX(e)
	Local vy:Float = EntityY(e)
	Local vz:Float = EntityZ(e)

	RotateEntity(e, VectorPitch(vx, vy, vz), VectorYaw(vx, vy, vz), EntityRoll(e))
	
End Function

Function Normalize:Float(value:Float = 128.0, value_min:Float = 0.0, value_max:Float = 255.0, norm_min:Float = 0.0, norm_max:Float = 1.0)

	Return ((value - value_min) / (value_max - value_min)) * (norm_max - norm_min) + norm_min
	
End Function

Oh, by the way. My current planet looks a little more polished than this example but suffers from the same problem, it's a part of my current project (click image for large resolution)

Krischan

(Posted 2014) [#2]

Ok, solved it using quaternions. Now you can drive around on a sphere like in a car which really makes fun, woo-hoo! However, there is a small limitation in Blitz3D because of the "low" accuracy - a blackout or a stuttering effect at the poles.

In miniB3D I could avoid it increasing the accuracy to doubles and using a incredible small value (1.0/2^512 vs. 1.0/2^64). Credits for the nice and handy Quaternion rotation code goes to Markus2 from the german Blitzforum.

Blitz3D:

Graphics3D 1024,768,32,2

SetBuffer BackBuffer()

Const movespeed#=0.25
Const turnspeed#=1.0

Global planet=CreateSphere(32)
EntityTexture planet,LoadTexture("earth.jpg")

Local map = LoadImage("earth.jpg")
ResizeImage(map, 400, 200)

Global pivot=CreatePivot()

Global cube=CreateCube(pivot)
ScaleMesh cube,0.002,0.002,0.002
PositionEntity cube,0,0,-1
EntityFX cube,1
EntityColor cube,255,0,0

Global cube2=CreateCube(cube)
ScaleEntity cube2,0.001,0.001,0.001
PositionEntity cube2,0,0.002,0
EntityFX cube2,1
EntityColor cube2,0,255,0

Global cam=CreateCamera(cube)
PositionEntity cam,0,0,0
RotateEntity cam,0,0,0
TranslateEntity cam,0,0,-0.2
CameraRange cam,0.01,100

Local q0.Quat = New Quat
Local q1.Quat = New Quat
Local q2.Quat = New Quat
Local q3.Quat = New Quat

EulerToQuat q0, 0,0,0

While Not KeyHit(1)
	
	TranslateEntity cam,0,0,((MouseDown(1)-MouseDown(2))*0.001)
	
	If KeyDown(17) Or KeyDown(200) Then EulerToQuat q1, movespeed,0,0 :MultiplyQuat(q0,q0,q1)	; Forward
	If KeyDown(31) Or KeyDown(208) Then EulerToQuat q1,-movespeed,0,0 :MultiplyQuat(q0,q0,q1)	; Backward
	
	If KeyDown(30) Or KeyDown(203) Then EulerToQuat q3,0,0, turnspeed :MultiplyQuat(q0,q0,q3)	; Turn left
	If KeyDown(32) Or KeyDown(205) Then EulerToQuat q3,0,0,-turnspeed :MultiplyQuat(q0,q0,q3)	; Turn right
	
	If KeyDown(18) Then EulerToQuat q2,0, movespeed,0 :MultiplyQuat(q0,q0,q2)					; Sidestep left
	If KeyDown(16) Then EulerToQuat q2,0,-movespeed,0 :MultiplyQuat(q0,q0,q2)					; Sidestep right
	
	QuatToEntity q0,pivot,0
	
	RenderWorld
	
	; calculate coordinates
	TFormPoint 0, 0, 0, cam, planet
	Local lat# = ATan2(Sqr(TFormedX() ^ 2 + TFormedZ() ^ 2), + TFormedY()) - 90
	Local lon# = ATan2(TFormedX(), -TFormedZ())
	If lat < - 90 Then lat = 90 Else If lat > 90 Then lat = 90
	If lon < - 180 Then lon = 180 Else If lon > 180 Then lon = 180
	
	; format coordinates
	Local lattext$, lontext$
	If lon >= 0 Then lontext = Int(Abs(lon)) + "E" Else lontext = Int(Abs(lon)) + "W"
	If lat <= 90 Then lattext = Int(Abs(lat)) + "N" Else lattext = Int(Abs(lat)) + "S"
	
	; minimap
	DrawImage map, 0, 0
	
	; draw player position
	Color 255, 0, 0
	Local x# = Normalize(lon, -180, 180, 0, 400 - 1)
	Local y# = Normalize(lat, -90, 90, 0, 200 - 1)
	Oval x - 2.5, y - 2.5, 5, 5
	
	; coordinates
	Color 255, 255, 255
	Text 0,200,"Position: " + lattext + "x" + lontext
	
	Flip
	
Wend

End

Function Normalize#(value# = 128.0, value_min# = 0.0, value_max# = 255.0, norm_min# = 0.0, norm_max# = 1.0)
	
	Return ((value - value_min) / (value_max - value_min)) * (norm_max - norm_min) + norm_min
	
End Function

Type Quat
	
	Field w#, x#, y#, z#
	
End Type

; Change these constants if you notice slips in accuracy
Const QuatToEulerAccuracy# = 1.0/2^64

;----------------------------------------------------------------------------

Function EulerToQuat(out.Quat, pitch#,yaw#,roll#)
	
	; convert a Rotation to a Quat
	; NB roll is inverted due to change in handedness of coordinate systems
	Local cr# = Cos(-roll/2)
	Local cp# = Cos(pitch/2)
	Local cy# = Cos(yaw/2)
	
	Local sr# = Sin(-roll/2)
	Local sp# = Sin(pitch/2)
	Local sy# = Sin(yaw/2)
	
	; These variables are only here to cut down on the number of multiplications
	Local cpcy# = cp * cy
	Local spsy# = sp * sy
	Local spcy# = sp * cy
	Local cpsy# = cp * sy
	
	; Generate the output quat
	out\w = cr * cpcy + sr * spsy
	out\x = sr * cpcy - cr * spsy
	out\y = cr * spcy + sr * cpsy
	out\z = cr * cpsy - sr * spcy
	
End Function

Function QuatToEntity(src.Quat,Entity,RotGlobal=0)
	
	; convert a Quat to a Rotation
	
	Local sint#, cost#, sinv#, cosv#, sinf#, cosf#
	Local cost_temp#
	
	sint = (2 * src\w * src\y) - (2 * src\x * src\z)
	cost_temp = 1.0 - (sint * sint)
	
	If Abs(cost_temp) > QuatToEulerAccuracy
		
		cost = Sqr(cost_temp)
		
	Else
		
		cost = 0
		
	EndIf
	
	If Abs(cost) > QuatToEulerAccuracy
		
		sinv = ((2 * src\y * src\z) + (2 * src\w * src\x)) / cost
		cosv = (1 - (2 * src\x * src\x) - (2 * src\y * src\y)) / cost
		sinf = ((2 * src\x * src\y) + (2 * src\w * src\z)) / cost
		cosf = (1 - (2 * src\y * src\y) - (2 * src\z * src\z)) / cost
		
	Else
		
		sinv = (2 * src\w * src\x) - (2 * src\y * src\z)
		cosv = 1 - (2 * src\x * src\x) - (2 * src\z * src\z)
		sinf = 0
		cosf = 1
		
	EndIf
	
	; Generate the output rotation, inverted due to change in handedness of coordinate system
	RotateEntity Entity,ATan2(sint, cost),ATan2(sinf, cosf),-ATan2(sinv, cosv),RotGlobal
	
	; fix for north/south pole blackout, results in a little stutter instead of complete rotation madness
	;If ATan2(sint, cost)<>"NaN" Then RotateEntity Entity,ATan2(sint, cost),ATan2(sinf, cosf),-ATan2(sinv, cosv),RotGlobal
	
	
End Function

Function MultiplyQuat(result.Quat, q1.Quat, q2.Quat)
	
	; result will be the same rotation as doing q1 then q2 (order matters!)
	
	Local a#, b#, c#, d#, e#, f#, g#, h#
	
	a = (q1\w + q1\x) * (q2\w + q2\x)
	b = (q1\z - q1\y) * (q2\y - q2\z)
	c = (q1\w - q1\x) * (q2\y + q2\z)
	d = (q1\y + q1\z) * (q2\w - q2\x)
	e = (q1\x + q1\z) * (q2\x + q2\y)
	f = (q1\x - q1\z) * (q2\x - q2\y)
	g = (q1\w + q1\y) * (q2\w - q2\z)
	h = (q1\w - q1\y) * (q2\w + q2\z)
	
	result\w = b + (-e - f + g + h) / 2.0
	result\x = a - ( e + f + g + h) / 2.0
	result\y = c + ( e - f + g - h) / 2.0
	result\z = d + ( e - f - g + h) / 2.0
	
End Function

MiniB3D:

SuperStrict

Framework sidesign.minib3d

Graphics3D 1024, 768, 32, 2

Const movespeed:Float = 0.25
Const turnspeed:Float = 1.0

Global planet:TMesh = CreateSphere(32)
EntityTexture planet, LoadTexture("earth.jpg")

Local map:TPixmap = LoadPixmap("earth.jpg")
map = ResizePixmap(map, 400, 200)

Global pivot:TPivot = CreatePivot()

Global cube:TMesh = CreateCube(pivot)
ScaleMesh cube, 0.002, 0.002, 0.002
PositionEntity cube, 0, 0, -1
EntityFX cube, 1
EntityColor cube, 255, 0, 0

Global cube2:TMesh = CreateCube(cube)
ScaleEntity cube2, 0.001, 0.001, 0.001
PositionEntity cube2, 0, 0.002, 0
EntityFX cube2, 1
EntityColor cube2, 0, 255, 0

Global cam:TCamera = CreateCamera(cube)
PositionEntity cam, 0, 0, 0
RotateEntity cam, 0, 0, 0
TranslateEntity cam, 0, 0, -0.5
CameraRange cam, 0.01, 100

Local q0:TQuat = New TQuat
Local q1:TQuat = New TQuat
Local q2:TQuat = New TQuat
Local q3:TQuat = New TQuat

EulerToQuat q0, 0, 0, 0

While Not AppTerminate()

	If KeyHit(KEY_ESCAPE) Then End
	
	TranslateEntity cam, 0, 0, ((MouseDown(1) - MouseDown(2)) * 0.001)
	
	If KeyDown(KEY_W) Or KeyDown(KEY_UP) Then EulerToQuat q1, movespeed, 0, 0;MultiplyQuat(q0, q0, q1)		' Forward
	If KeyDown(KEY_S) Or KeyDown(KEY_DOWN) Then EulerToQuat q1, -movespeed, 0, 0;MultiplyQuat(q0, q0, q1)	' Backward
	
	If KeyDown(KEY_A) Or KeyDown(KEY_LEFT) Then EulerToQuat q3, 0, 0, turnspeed;MultiplyQuat(q0, q0, q3)	' Turn left
	If KeyDown(KEY_D) Or KeyDown(KEY_RIGHT) Then EulerToQuat q3, 0, 0, -turnspeed;MultiplyQuat(q0, q0, q3)	' Turn right
	
	If KeyDown(KEY_E) Then EulerToQuat q2, 0, movespeed, 0;MultiplyQuat(q0, q0, q2)							' Sidestep right
	If KeyDown(KEY_Q) Then EulerToQuat q2, 0, -movespeed, 0;MultiplyQuat(q0, q0, q2)						' Sidestep left
	
	QuatToEntity q0, pivot, 0
	
	RenderWorld
	
	BeginMax2D()

		' calculate coordinates
		TFormPoint 0, 0, 0, cam, planet
		Local lat:Float = ATan2(Sqr(TFormedX() ^ 2 + TFormedZ() ^ 2), + TFormedY()) - 90
		Local lon:Float = ATan2(TFormedX(), -TFormedZ())
		If lat < - 90 Then lat = 90 Else If lat > 90 Then lat = 90
		If lon < - 180 Then lon = 180 Else If lon > 180 Then lon = 180

		' format coordinates
		Local lattext:String, lontext:String
		If lon >= 0 Then lontext = Int(Abs(lon)) + "E" Else lontext = Int(Abs(lon)) + "W"
		If lat <= 90 Then lattext = Int(Abs(lat)) + "N" Else lattext = Int(Abs(lat)) + "S"
	
		' minimap
		DrawPixmap map, 0, 0

		' draw player position
		SetColor 255, 0, 0
		Local x:Float = Normalize(lon, -180, 180, 0, 400 - 1)
		Local y:Float = Normalize(lat, -90, 90, 0, 200 - 1)
		DrawOval x - 2.5, y - 2.5, 5, 5

		' coordinates
		SetColor 255, 255, 255
		DrawText "Position: " + lattext + "x" + lontext, 0, 200
						
	EndMax2D()
	
	Flip
	
Wend

End

Function Normalize:Float(value:Float = 128.0, value_min:Float = 0.0, value_max:Float = 255.0, norm_min:Float = 0.0, norm_max:Float = 1.0)

	Return ((value - value_min) / (value_max - value_min)) * (norm_max - norm_min) + norm_min
	
End Function

Type TQuat
	
	Field w:Double, x:Double, y:Double, z:Double
	
End Type

' Change these constants if you notice slips in accuracy
Const QuatToEulerAccuracy:Double = 1.0 / 2 ^ 512

'----------------------------------------------------------------------------

Function EulerToQuat(out:TQuat, pitch:Double, yaw:Double, roll:Double)
	
	' convert a Rotation to a Quat
	' NB roll is inverted due to change in handedness of coordinate systems
	Local cr:Double = Cos(-roll / 2)
	Local cp:Double = Cos(pitch / 2)
	Local cy:Double = Cos(yaw / 2)
	
	Local sr:Double = Sin(-roll / 2)
	Local sp:Double = Sin(pitch / 2)
	Local sy:Double = Sin(yaw / 2)
	
	' These variables are only here to cut down on the number of multiplications
	Local cpcy:Double = cp * cy
	Local spsy:Double = sp * sy
	Local spcy:Double = sp * cy
	Local cpsy:Double = cp * sy
	
	' Generate the output quat
	out.w = cr * cpcy + sr * spsy
	out.x = sr * cpcy - cr * spsy
	out.y = cr * spcy + sr * cpsy
	out.z = cr * cpsy - sr * spcy
	
End Function

Function QuatToEntity(src:TQuat, Entity:TEntity, RotGlobal:Int = 0)
	
	' convert a Quat to a Rotation
	
	Local sint:Double, cost:Double, sinv:Double, cosv:Double, sinf:Double, cosf:Double
	Local cost_temp:Double
	
	sint = (2 * src.w * src.y) - (2 * src.x * src.z)
	cost_temp = 1.0 - (sint * sint)
	
	If Abs(cost_temp) > QuatToEulerAccuracy
		
		cost = Sqr(cost_temp)
		
	Else
		
		cost = 0
		
	EndIf
	
	If Abs(cost) > QuatToEulerAccuracy
		
		sinv = ((2 * src.y * src.z) + (2 * src.w * src.x)) * 1.0 / cost
		cosv = (1 - (2 * src.x * src.x) - (2 * src.y * src.y)) * 1.0 / cost
		sinf = ((2 * src.x * src.y) + (2 * src.w * src.z)) * 1.0 / cost
		cosf = (1 - (2 * src.y * src.y) - (2 * src.z * src.z)) * 1.0 / cost
		
	Else
		
		sinv = (2 * src.w * src.x) - (2 * src.y * src.z)
		cosv = 1 - (2 * src.x * src.x) - (2 * src.z * src.z)
		sinf = 0
		cosf = 1
		
	EndIf
	
	' Generate the output rotation, inverted due to change in handedness of coordinate system
	RotateEntity Entity, ATan2(sint, cost), ATan2(sinf, cosf), -ATan2(sinv, cosv), RotGlobal
	
End Function

Function MultiplyQuat(result:TQuat, q1:TQuat, q2:TQuat)
	
	' result will be the same rotation as doing q1 then q2 (order matters!)
	
	Local a:Double, b:Double, c:Double, d:Double, e:Double, f:Double, g:Double, h:Double
	
	a = (q1.w + q1.x) * (q2.w + q2.x)
	b = (q1.z - q1.y) * (q2.y - q2.z)
	c = (q1.w - q1.x) * (q2.y + q2.z)
	d = (q1.y + q1.z) * (q2.w - q2.x)
	e = (q1.x + q1.z) * (q2.x + q2.y)
	f = (q1.x - q1.z) * (q2.x - q2.y)
	g = (q1.w + q1.y) * (q2.w - q2.z)
	h = (q1.w - q1.y) * (q2.w + q2.z)
	
	result.w = b + (-e - f + g + h) / 2.0
	result.x = a - (e + f + g + h) / 2.0
	result.y = c + (e - f + g - h) / 2.0
	result.z = d + (e - f - g + h) / 2.0
	
End Function

Rroff

(Posted 2014) [#3]

IIRC (been awhile since I played with it) for B3D and large scale stuff you have to reset so the player position is around 0,0,0 for rendering - I think if you want you can do all the update stuff with the world in its normal position especially if your using a 3rd party physics engine then move it temporary when rendering then move it back. I've not really played with it too extensively though.

RGR	(Posted 2014) [#4]

Edit..