Vehicle's attitude / orientation code

Hi!

I made a python code to get a vechile’s, like car, tank, boat or airplane, attitude / orientation figures in planetary gravity field.

The code assumes, the vehicle’s forward axis is y.
The figures are given in RADIANS as following:

  • Pitch (nose up/down), x-axis
  • Roll (left / right), y-axis
  • Compass or yaw (left / right), z-axis

# Vehicle's global attitude / orintation code by Xjazz, 041212. Use freely

import bge
from math import asin, cos

def adjust_roll2(ref, val): # ROLL 
    if ref  > 0.0: return val
    if (ref  < 0.0) and (val > 0.0) : return 3.14159265359 - val
    if (ref  < 0.0) and (val < 0.0) : return -3.14159265359 - val
    if (ref  < 0.0) and (val == 0.0) : return -3.14159265359

def adjust_heading2(ref, val): # COMPASS HEADING
    if (val > 0.0) and (ref < 0.0): return 3.14159265359 - val
    elif (val < 0.0) and (ref> 0.0): return 6.28318530718 + val
    elif (val < 0.0) and (ref< 0.0): return 3.14159265359- val
    elif (val == 0.0) and (ref < 0.0): return 3.14159265359
    else: return val

def main(cont):
    own = cont.owner
    WO = own.worldOrientation  
    # PITCH +-/- 1.570796327 radians. 0.0 = nose level
    own['ox'] = asin(WO[2][1] / 1.0)    
    # ROLL +/-3.141592653590 radians. 0.0 = wings level
    own['oy'] = adjust_roll2(WO[2][2], asin(WO[2][0] / cos(own['ox'] )))    
    # COMPASS HEADING 0.0/6.2 = N, 1.57 = E, 3.14 = S, 4.71 = W
    own['oz'] = adjust_heading2(WO[1][1], asin(WO[0][1] / cos(own['ox'] )))   

(edit)
I added a sample .blend file, which is presenting airplane’s artificial horizon and compass.

Attachments

Orientation_Attitude_b264.blend (434 KB)

Hi!

Any feedback concerning the usability of the code?

Is it giving a proper attitude figures in your specific application?
Advices to improve the code?

Thanks

Hi!

I got some very valuable feedback concerning the coding in general.

This code is now packed in to one function and therefore it is easier to use.


def  vehicle_attitude(WorldOrientation):
    '''
    This function returns a list of object's PITCH, ROLL and COMPASS/ LATERAL HEADING values, 
    when feeded with the object’s world orientation 3x3 Matrix.

    NOTE! A cos() and asin() functions from the math module are neened.
    Add following line to your main script/module: from math import asin, cos

    The function assumes the axies are point following: +x = right, +y = forward, +z = up.
    
    Output value ranges:
    X-AXIS = PITCH(nose up/down):  +/- 1.57radians. 0.0 = nose level.
    Y-AXIS = ROLL(left/right): +/- 1.57radians. 0.0 = wings level.
    Z-AXIS = COMPASS(lateral heading): 0 - 6.2 radians. 0.0/6.2 = N, 1.57 = E, 3.14 = S, 4.71 = W.
    '''

    # PITCH
    attitude_x = asin(WorldOrientation[2][1] / 1.0)
    attitude_xt  = cos(attitude_x)

    # ROLL(adjusted)
    attitude_yt = asin(WorldOrientation[2][0] / attitude_xt)
    if WorldOrientation[2][2] > 0.0: attitude_y =  attitude_yt
    if (WorldOrientation[2][2] < 0.0) and (attitude_yt > 0.0): attitude_y =  3.14159265359 - attitude_yt
    if (WorldOrientation[2][2] < 0.0) and (attitude_yt < 0.0): attitude_y = -3.14159265359 - attitude_yt
    if (WorldOrientation[2][2]  < 0.0) and (attitude_yt == 0.0): attitude_y =  -3.14159265359

    # COMPASS / HEADING (adjusted)
    attitude_zt = asin(WorldOrientation[0][1] / attitude_xt)
    if (attitude_zt > 0.0) and (WorldOrientation[1][1] < 0.0): attitude_z = 3.14159265359 - attitude_zt 
    elif (attitude_zt < 0.0) and (WorldOrientation[1][1] > 0.0): attitude_z = 6.28318530718 + attitude_zt
    elif (attitude_zt < 0.0) and (WorldOrientation[1][1] < 0.0): attitude_z = 3.14159265359- attitude_zt
    elif (attitude_zt == 0.0) and (WorldOrientation[1][1] < 0.0): attitude_z = 3.14159265359
    else: attitude_z = attitude_zt

    return [attitude_x, attitude_y, attitude_z]

Looks much better now. And it includes good description what it does.

Thanks
Monster