Aligning a Mesh to a picked Surface Normal

BlitzMax Forums/MiniB3D Module/Aligning a Mesh to a picked Surface Normal

Krischan

(Posted 2016) [#1]

I want to align a rover model to a polygon surface to drive around on a planet surface. Sounds easy but isn't in minib3d, in Blitz3D I solved this using the AligntoVector command. The planet is pickable and a vertical Linepick gets the surface normal below the rover. The vertical position is no problem but when it comes to the correct alignment of the rover all my algorithms fail and I could need some help here. The rover can move freely in all directions but should stay on the planet surface.

Here is a WIP screenshot

And here is an example code to play with. The cone should align with the quad according to its picked normal but stops suddenly and continues aligning a little bit later again. I have absolute no idea why. I have an example function "AlignToNormal" and included my Quaternion mini library and I assume the solution lies in using quaternions here. Oh, and I want to use vanilla miniB3D if this is possible with it.

SuperStrict

Framework sidesign.minib3d

Graphics3D 1024, 600, 32, 2

Const QuatToEulerAccuracy:Double = 1.0 / 2 ^ 32

Local base:TMesh = CreateQuad()
EntityFX base, 1 + 16
PositionEntity base, 0, 0, 0
RotateEntity base, 60, -30, 0
EntityPickMode base, 2
UpdateNormals base

Local player:TMesh = CreateCone(32)
RotateMesh player, 90, 0, 0
ScaleEntity player, 0.25, 0.25, 0.25
EntityColor player, 0, 255, 0
PositionEntity player, 0, 0, 0

Local cam:TCamera = CreateCamera()
PositionEntity cam, -2, -1, -1
PointEntity cam, player

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

Wireframe 0

While Not KeyHit(KEY_ESCAPE)

	TurnEntity base, 0.5, 1, 0.25

	LinePick(0, 10, 0, 0, -20, 0)
		
	PositionEntity player, 0, PickedY() + 0.2, 0
	
	Local nx:Float = PickedNX()
	Local ny:Float = PickedNY()
	Local nz:Float = PickedNZ()
			
	AlignToNormal(player, nx, ny, nz)
					
	If KeyDown(KEY_W) Then EulerToQuat q1, 1, 0, 0;MultiplyQuat(q0, q0, q1)
	If KeyDown(KEY_S) Then EulerToQuat q1, -1, 0, 0;MultiplyQuat(q0, q0, q1)
	If KeyDown(KEY_A) Then EulerToQuat q2, 0, 1, 0;MultiplyQuat(q0, q0, q2)
	If KeyDown(KEY_D) Then EulerToQuat q2, 0, -1, 0;MultiplyQuat(q0, q0, q2)
	If KeyDown(KEY_Q) Then EulerToQuat q3, 0, 0, 1;MultiplyQuat(q0, q0, q3)
	If KeyDown(KEY_E) Then EulerToQuat q3, 0, 0, -1;MultiplyQuat(q0, q0, q3)
	QuatToEntity q0, player, 0

	UpdateWorld
	RenderWorld
	
	Text 0, 0, "Pitch: " + EntityPitch(player)
	Text 0, 15, "Yaw:   " + EntityYaw(player)
	Text 0, 30, "Roll:  " + EntityRoll(player)
	
	Text 0, 50, "NX: " + nx
	Text 0, 65, "NY: " + ny
	Text 0, 80, "NZ: " + nz
			
	Flip

Wend

End

Function Normalize:Float(value:Float = 128.0, vmin:Float = 0.0, vmax:Float = 255.0, nmin:Float = 0.0, nmax:Float = 1.0)
    
	Return ((value:Float - vmin:Float) / (vmax:Float - vmin:Float)) * (nmax:Float - nmin:Float) + nmin:Float
    
End Function

Function AlignToNormal(e:TEntity, nx:Float, ny:Float, nz:Float)
	
	Local yaw:Float, pitch:Float, roll:Float
	Local x1:Float, y1:Float, z1:Float
	Local x2:Float, y2:Float
	
	yaw = -ATan2(nx, nz)
	x1 = nz * Sin(yaw) + nx * Cos(yaw)
	y1 = ny
	z1 = nz * Cos(yaw) - nx * Sin(yaw)
	
	pitch = -ATan2(y1, z1)
	x2 = x1
	y2 = y1 * Cos(pitch) - z1 * Sin(pitch)
		
	roll = -ATan2(x2, y2)

	If ny < 0 roll:+180 Else If ny > 180 roll:-180
	
	RotateEntity e, pitch, yaw, roll
	
End Function

Function CreateQuad:TMesh(parent:TEntity = Null, r:Int = 255, g:Int = 255, b:Int = 255, a:Float = 1.0)
	
	Local v1:Int, v2:Int, v3:Int, v4:Int
	
	Local mesh:TMesh = CreateMesh(parent)
	
	Local surf:TSurface = CreateSurface(mesh)
	
	v1 = AddVertex(surf, -1, 1, 0, 0, 0)
	v2 = AddVertex(surf, 1, 1, 0, 1, 0)
	v3 = AddVertex(surf, -1, -1, 0, 0, 1)
	v4 = AddVertex(surf, 1, -1, 0, 1, 1)

	VertexColor surf, v1, r, g, b, a
	VertexColor surf, v3, r, g, b, a
	VertexColor surf, v2, r, g, b, a
	VertexColor surf, v4, r, g, b, a

	AddTriangle(surf, 0, 1, 2)
	AddTriangle(surf, 3, 2, 1)
			
	Return mesh
	
End Function

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

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
	
	Local a1:Float = ATan2(sint, cost)
	Local a2:Float = ATan2(sinf, cosf)
	Local a3:Float = -ATan2(sinv, cosv)
	
	if a1<>Null then RotateEntity Entity, a1, a2, a3, 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

Function Turn(ent:TEntity, X:Float, Y:Float, Z:Float, Glob:Int = False)

	Local Pitch:Float = 0.0
	Local Yaw:Float = 0.0
	Local Roll:Float = 0.0
	
	Local Quat:TQuaternion = EulerToQuat2(0.0, 0.0, 0.0)
	Local Turn_Quat:TQuaternion = EulerToQuat2(0.0, 0.0, 0.0)
	
	If Glob=False
	
		Quat = EulerToQuat2(EntityPitch(Ent, True), EntityYaw(Ent, True), EntityRoll(Ent, True))
		Turn_Quat = EulerToQuat2(X, Y, Z)
		Quat = MultiplyQuats2(Quat, Turn_Quat)
		Quat = NormalizeQuat2(Quat)
		QuatToEuler2( Quat.x, Quat.y, Quat.z, Quat.w, Pitch, Yaw, Roll )
		RotateEntity Ent, Pitch, Yaw, Roll
	Else
	
		RotateEntity Ent, EntityPitch( Ent )+X, EntityYaw( Ent )+Y, EntityRoll( Ent )+Z
		
	EndIf
	
End Function

Function EulerToQuat2:TQuaternion(pitch:Float, yaw:Float, roll:Float)

	Local cr:Float=Cos(-roll/2.0)
	Local cp:Float=Cos(pitch/2.0)
	Local cy:Float=Cos(yaw/2.0)
	Local sr:Float=Sin(-roll/2.0)
	Local sp:Float=Sin(pitch/2.0)
	Local sy:Float=Sin(yaw/2.0)
	Local cpcy:Float=cp*cy
	Local spsy:Float=sp*sy
	Local spcy:Float = sp * cy
	Local cpsy:Float=cp*sy
	
	Local q:TQuaternion=New TQuaternion
	
	q.w:Float=cr*cpcy+sr*spsy
	q.x:Float=sr*cpcy-cr*spsy
	q.y:Float=cr*spcy+sr*cpsy
	q.z:Float=cr*cpsy-sr*spcy
	
	Return q
	
End Function

Function QuatToEuler2(x:Float,y:Float,z:Float,w:Float,pitch:Float Var,yaw:Float Var,roll:Float Var)

	Local sint:Float=(2.0*w*y)-(2.0*x*z)
	Local cost_temp:Float=1.0-(sint*sint)
	Local cost:Float

	If Abs(cost_temp)>QuatToEulerAccuracy

		cost=Sqr(cost_temp)

		Else

		cost=0.0

	EndIf

	Local sinv:Float,cosv:Float,sinf:Float,cosf:Float
	
	If Abs(cost)>QuatToEulerAccuracy
	
		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
	
	pitch=ATan2(sint,cost)
	yaw=ATan2(sinf,cosf)
	roll=-ATan2(sinv,cosv)
	
End Function

Function MultiplyQuats2:TQuaternion(q1:TQuaternion, q2:TQuaternion)

	Local q:TQuaternion=New TQuaternion
	
	q.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z
	q.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y
	q.y = q1.w*q2.y + q1.y*q2.w + q1.z*q2.x - q1.x*q2.z
	q.z = q1.w*q2.z + q1.z*q2.w + q1.x*q2.y - q1.y*q2.x

	Return q

End Function

Function NormalizeQuat2:TQuaternion(q:TQuaternion)

	Local uv:Float=Sqr(q.w*q.w+q.x*q.x+q.y*q.y+q.z*q.z)

	q.w=q.w/uv
	q.x=q.x/uv
	q.y=q.y/uv
	q.z=q.z/uv

	Return q

End Function

KronosUK

(Posted 2016) [#2]

I always found MadJacks code in the archive quite good for this. You can use linepicks instead to get vertical height of terrain although it does require 4 linepicks per calc.

Vehicle pitch and roll without child/pivot entities

Krischan

(Posted 2016) [#3]

Kronos, that's an excellent idea. I've already implemented this in my sandbox and it works like a charme - the spotlight attached to the rover now points correct downwards the crater hill and the rover drives around like a dune buggy, cool :-) Thanks!

Here is the sandbox with the new "AligntoNormal" function which does the trick:

SuperStrict

Framework sidesign.minib3d

Graphics3D 1024, 600, 32, 2

Global base:TMesh = CreateQuad()
EntityFX base, 1 + 16
PositionEntity base, 0, 0, 0
RotateMesh base, 90, 0, 0
RotateEntity base, 30, 45, 0
EntityPickMode base, 2
UpdateNormals base

Global player:TMesh = CreateCone(32)
ScaleEntity player, 0.125, 0.125, 0.125
EntityColor player, 0, 255, 0
PositionMesh player, 0, 0.5, 0

Local cam:TCamera = CreateCamera()
PositionEntity cam, -1, 2.5, -2
PointEntity cam, base
CameraRange cam, 0.1, 10

Global m1:TMesh = CreateSphere(2)
Global m2:TMesh = CreateSphere(2)
Global m3:TMesh = CreateSphere(2)
Global m4:TMesh = CreateSphere(2)
ScaleEntity m1, 0.1, 0.1, 0.1
ScaleEntity m2, 0.1, 0.1, 0.1
ScaleEntity m3, 0.1, 0.1, 0.1
ScaleEntity m4, 0.1, 0.1, 0.1
EntityFX m1, 1
EntityFX m2, 1
EntityFX m3, 1
EntityFX m4, 1
EntityColor m1, 255, 0, 0
EntityColor m2, 255, 255, 0
EntityColor m3, 0, 255, 0
EntityColor m4, 0, 0, 255

'Wireframe 1

While Not KeyHit(KEY_ESCAPE)

	TurnEntity player, 0, (KeyDown(KEY_LEFT)) - (KeyDown(KEY_RIGHT)), 0

	TurnEntity base, 0, 0.5, 0
	
	AlignToNormal(player, 0.5)
		
	RenderWorld
						
	Flip

Wend

End

Function AlignToNormal(e:TMesh, d:Float = 0.25, vy:Float = 1000.0)

	Local x:Float = EntityX(e)
	Local y:Float = EntityY(e)
	Local z:Float = EntityZ(e)
	
	Local yaw:Float = EntityYaw(e)
	
	Local an:Float ' New Yaw Angle
	Local xn:Float ' New X coordinate
	Local zn:Float ' New Z coordinate
	
	Local fh:Float ' Front Height
	Local bh:Float ' Back Height
	Local lh:Float ' Left Height
	Local rh:Float ' Right Height
		
	' Front
	an = yaw - 90
	xn = x + Cos(an) * d
	zn = z + Sin(an) * d
	LinePick(xn, y + vy, zn, 0, -vy * 2, 0)
	fh = PickedY()
	PositionEntity m1, xn, fh, zn

	' Back
	an = yaw + 90
	xn = x + Cos(an) * d
	zn = z + Sin(an) * d
	LinePick(xn, y + vy, zn, 0, -vy * 2, 0)
	bh = PickedY()
	PositionEntity m2, xn, bh, zn

	' Left
	an = yaw
	xn = x + Cos(an) * d
	zn = z + Sin(an) * d
	LinePick(xn, y + vy, zn, 0, -vy * 2, 0)
	lh = PickedY()
	PositionEntity m3, xn, lh, zn

	' Right
	an = yaw + 180
	xn = x + Cos(an) * d
	zn = z + Sin(an) * d
	LinePick(xn, y + vy, zn, 0, -vy * 2, 0)
	rh = PickedY()
	PositionEntity m4, xn, rh, zn
	
	' Pitch calculation
	Local pitchx:Float = d * 2
	Local pitchy:Float = bh - fh
	Local pitch:Float = ATan2(pitchx, pitchy)

	' Roll calculation
	Local rollx:Float = d * 2
	Local rolly:Float = rh - lh
	Local roll:Float = ATan2(rollx, rolly)
		
	' Rotate player
	RotateEntity e, pitch - 90, yaw, roll - 90
	
	' reposition player ot weighted height
	PositionEntity e, x, ((fh + bh + lh + rh) / 4.0), z
					
End Function

Function Normalize:Float(value:Float = 128.0, vmin:Float = 0.0, vmax:Float = 255.0, nmin:Float = 0.0, nmax:Float = 1.0)
    
	Return ((value:Float - vmin:Float) / (vmax:Float - vmin:Float)) * (nmax:Float - nmin:Float) + nmin:Float
    
End Function

Function CreateQuad:TMesh(parent:TEntity = Null, r:Int = 255, g:Int = 255, b:Int = 255, a:Float = 1.0)
	
	Local v1:Int, v2:Int, v3:Int, v4:Int
	
	Local mesh:TMesh = CreateMesh(parent)
	
	Local surf:TSurface = CreateSurface(mesh)
	
	v1 = AddVertex(surf, -1, 1, 0, 0, 0)
	v2 = AddVertex(surf, 1, 1, 0, 1, 0)
	v3 = AddVertex(surf, -1, -1, 0, 0, 1)
	v4 = AddVertex(surf, 1, -1, 0, 1, 1)

	VertexColor surf, v1, r, g, b, a
	VertexColor surf, v3, r, g, b, a
	VertexColor surf, v2, r, g, b, a
	VertexColor surf, v4, r, g, b, a

	AddTriangle(surf, 0, 1, 2)
	AddTriangle(surf, 3, 2, 1)
			
	Return mesh
	
End Function

In my sandbox, the function is called like this (and the terrain below is set to Entitypickmode 2 = polygon)

' Rover Movement
moving = KeyDown(KEY_UP) - KeyDown(KEY_DOWN)
turning = KeyDown(KEY_LEFT) - KeyDown(KEY_RIGHT)
	
TurnEntity rover, 0, turning, 0
MoveEntity rover, 0, 0, moving
AlignToNormal(rover, 2.0)

Hezkore

(Posted 2016) [#4]

I see you've already got it working, but is this perhaps of any help? - http://www.blitzbasic.com/codearcs/codearcs.php?code=2782
Or is it what you're basically doing already?