path: root/math_funcs.py

import math
from mathutils import Matrix

# file containning math related functions

# function to return a 0 filled matrix 3x3
def get_zero_mat3x3():
  
  return Matrix(([0, 0, 0], [0, 0, 0], [0, 0, 0]))
      
# function to return a 0 filled matrix 4x4
def get_zero_mat4x4():
  
  return Matrix(([0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]))

# function to return an identity matrix 3x3
def get_id_mat3x3():
  
  return Matrix(([1, 0, 0], [0, 1, 0], [0, 0, 1]))
      
# function to return an identity matrix 4x4
def get_id_mat4x4():
  
  return Matrix(([1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]))  

# calc_scale_matrix function
# function to build the scale matrix
def calc_scale_matrix(x_scale, y_scale, z_scale):

    scale_matrix = Matrix(( [x_scale, 0, 0, 0],
                            [0, y_scale, 0, 0],
                            [0, 0, z_scale, 0],
                            [0, 0, 0, 1]
                              ))
                        
    return scale_matrix

# calc_rotation_matrix function
# function to calculate the rotation matrix
# for a Extrinsic Euler XYZ system (radians)
def calc_rotation_matrix(x_angle, y_angle, z_angle):

    x_rot_mat = Matrix(([1, 0, 0, 0],
                        [0, math.cos(x_angle), -math.sin(x_angle), 0],
                        [0, math.sin(x_angle), math.cos(x_angle), 0],
                        [0, 0, 0, 1]))

    y_rot_mat = Matrix(([math.cos(y_angle), 0, math.sin(y_angle), 0],
                        [0, 1, 0, 0],

                        [-math.sin(y_angle), 0, math.cos(y_angle), 0],
                        [0, 0, 0, 1]))
                    
    z_rot_mat = Matrix(([math.cos(z_angle), -math.sin(z_angle), 0, 0],
                        [math.sin(z_angle), math.cos(z_angle), 0, 0],
                        [0, 0, 1, 0],
                        [0, 0, 0, 1]))
                        
    return z_rot_mat * y_rot_mat * x_rot_mat

# calc_translation_matrix function
# function to build the translation matrix
def calc_translation_matrix(x_translation, y_translation, z_translation):

    translation_matrix = Matrix(( [1, 0, 0, x_translation],
                                  [0, 1, 0, y_translation],
                                  [0, 0, 1, z_translation],
                                  [0, 0, 0, 1]
                                    ))
                        
    return translation_matrix

# interpolate function
# used to find a value in an interval with the specified mode.
# So that it is clear that the values are points that have 2
# coordinates I will treat the input as they are (x,y) points
# the function will either return m_x or m_y depending on
# which of the middle point values is provided to the function
# (set None to the variable going to be returned by the 
# function). Only linear interpolation is supported for now.
#
# l_x (float) --> left point X axis component
# l_y (float) --> left point Y axis component
# r_x (float) --> right point X axis component
# r_y (float) --> right point Y axis component
# m_x (float) --> middle point X axis component
# m_y (float) --> middle point Y axis component
# interp_type (string) --> "linear" for linear interpolation
def interpolate(l_x, l_y, r_x, r_y, m_x, m_y, interp_type):

  # variable to be returned
  result = 0
  
  # if right point does not exist (special case)
  # return l_y as the interpolation result
  if (r_x == None or r_y == None):
    result = l_y
    return result
  
  # m_x is the one to be returned
  if (m_x == None):
    
    # linear interpolation
    if (interp_type == "linear"):
      m_x = (((r_x - l_x) / (r_y - l_y)) * (m_y - r_y)) + r_x
      result = m_x
      
  # m_y is the one to be returned
  if (m_y == None):
    
    # linear interpolation
    if (interp_type == "linear"):
      m_y = (((r_y - l_y) / (r_x - l_x)) * (m_x - r_x)) + r_y
      result = m_y
    
  return result

# find_left_right function
# find the values and positions of the elements at the left and the
# right of the element in position pos on the anim_array
# elements will be used in the interpolate() function later
#
# anim_array (array of floats) --> anim property frame array its
#                                  length must the animation length
# pos (int) --> position of the animation property value to be
#               later interpolated in the anim_array array
def find_left_right(anim_array, pos):

  # create left/right variables
  l_val = 0
  l_val_pos = 0
  r_val = 0
  r_val_pos = 0
  
  # find near left value (has to exist)
  # read array from right to left
  for i in range(len(anim_array), -1, -1):
  # 
    # skip elements at the right of 
    # pos in anim_array
    if (i >= pos):
      continue
    
    # left value is found
    if (anim_array[i] != None):
      l_val = anim_array[i]
      l_val_pos = i
      break
  #
  
  ##############
  # special case
  # if pos is the last element position on
  # the array r_val and r_val_pos do not exist
  if (pos == (len(anim_array) - 1)):
    return [l_val_pos, l_val, None, None]
  
  # find near right value (might not exist)
  # read array from left to right
  for i in range(len(anim_array)):
    # skip elements at the left of 
    # pos in anim_array
    if (i <= pos):
      continue
    
    # right value is found
    if (anim_array[i] != None):
      r_val = anim_array[i]
      r_val_pos = i
      break
    
    # if no value is found at the end of 
    # the anim_array r_val and r_val_pos do not exist
    # (value does not change between the left value 
    # found and the end of the animation)
    if (i == (len(anim_array) - 1)):
      return [l_val_pos, l_val, None, None]
  
  # if all values are found, return them
  return [l_val_pos, l_val, r_val_pos, r_val]

# convert_angle_to_180 function
# function used by the convert_anim_rot_to_180 function
# to convert a single angle in its representation on
# the -180/+180 degree range (angles passed to the 
# function that are already in this range will be 
# returned without conversion)
#
# angle (float) --> angle to convert to the -180/+180
#                   degree range (angle is expected to
#                   be in degrees)
def convert_angle_to_180(angle):
#  
  # check if the angle really needs to be processed
  # i.e. is already inside the -180/+180 degree range
  if (angle >= -180 and angle <= 180):
   return angle
    
  # convert it otherwise
  
  # check if it is positive or negative
  # and set the opposite direction of the angle
  # if the angle is > 0 then its mesurement is clockwise (opposite is counter-clockwise)
  # if the angle is < 0 then its mesurement is counter-clockwise (opposite is clockwise)
  if (angle > 0):
    opposite_spin_dir = -1
  else: # it is negative
    opposite_spin_dir = 1
  
  # decrease the angle by 360 degrees until it
  # is in the -180/+180 degree interval
  while (abs(angle) > 180):
    angle = angle + (opposite_spin_dir * 360)
  
  return angle

# convert_anim_rot_to_180 function
# used to re-calculate a rotation animation on an axis
# so that angles used lay in between -180/180 degrees
# done to avoid rotation animation data loss when extracting
# said angles from a transformation matrix
# this function calls the convert_angle_to_180() function
# at the end of the function csv_keyframe_numbers is updated
# with the new frames to be injected into the animation
#
# example:
#
#          Original keyframes:
#          Frame 0  Frame 21  (2 keyframes)
#          0º       360º
#
#          Processed keyframes:
#          Frame 0  Frame 10  Frame 11  Frame 21  (4 keyframes)
#          0º       171.4º    -171.5º   0º
#
# rot_array (array of floats) --> bone rotation animation data
#                                 for a single axis
# csv_keyframe_numbers (array of ints) --> original keyframes
#                                          of the animation
#
# Note: function will have problems interpreting keyframes with
#       high rotation diferences if the number of frames in 
#       between said keyframes in lower than 2 times the spins
#       done in between those keyframe angles (thinking a fix)
def convert_rot_anim_to_180(rot_array, csv_keyframe_numbers):
  
  # temp rot array to store calculated values and keyframe position
  rot_array_cp = [[], []]
  
  # find the frames in which rot_array has values defined
  rot_array_kf = []
  for i in range(len(rot_array)):
    if (rot_array[i] != None):
      rot_array_kf.append(i)
  
  # loop through each consecutive pair of keyframes of the rot_array_kf array
  for i in range(len(rot_array_kf) - 1):
  #  
    # get left/right keyframe values and positions
    l_kf_pos = rot_array_kf[i]
    l_kf_val = rot_array[l_kf_pos]
    r_kf_pos =  rot_array_kf[i + 1]
    r_kf_val = rot_array[r_kf_pos]
    
    # append l_kf_val to rot_array_cp (converted)
    rot_array_cp[0].append(l_kf_pos)
    rot_array_cp[1].append(convert_angle_to_180(l_kf_val))
    
    # get the rotation direction
    if (r_kf_val > l_kf_val): # clockwise
      rot_direction = 1
    else:                     # counter-clockwise
      rot_direction = -1
    
    # advance -180/+180 (depending on the rotation direction)
    # and generate the middle keyframe values
    # angle is fixed to the l_kf_val's closest 360 degree multiple
    angle_val = l_kf_val - convert_angle_to_180(l_kf_val)
    while (abs(r_kf_val - angle_val) > 180):
    #  
      angle_val = angle_val + (rot_direction * 180)
      
      # find the frame (float) in which this value exists
      angle_pos = interpolate(l_kf_pos, l_kf_val, r_kf_pos, r_kf_val, None, angle_val, "linear")
      
      # find the 2 frames (integer) that are lower 
      # and upper limits of this angle_pos 
      lower_frame = int(angle_pos)
      upper_frame = int(angle_pos + 1)
      
      # interpolate to find the values on lower_frame and upper_frame      
      lower_frame_value = convert_angle_to_180(interpolate(l_kf_pos, l_kf_val, r_kf_pos, r_kf_val, lower_frame, None, "linear"))
      upper_frame_value = convert_angle_to_180(interpolate(l_kf_pos, l_kf_val, r_kf_pos, r_kf_val, upper_frame, None, "linear"))
      
      # append results to rot_array_cp
      
      # keyframes
      rot_array_cp[0].append(lower_frame)
      rot_array_cp[0].append(upper_frame)
      # values
      rot_array_cp[1].append(lower_frame_value)
      rot_array_cp[1].append(upper_frame_value)
  
  # add the new keyframe values to rot_array
  # on their respective frame position
  for i in range(len(rot_array_cp[0])):
    rot_array[rot_array_cp[0][i]] = rot_array_cp[1][i]
    
  # update the keyframes on csv_keyframe_numbers to store
  # the calculated keyframes numbers from rot_array_cp[0]
  # append those at the end of csv_keyframe_numbers
  for i in range(len(rot_array_cp[0])):
    value_found = False
    for j in range(len(csv_keyframe_numbers)):
      if (rot_array_cp[0][i] == csv_keyframe_numbers[j]):
        value_found = True
        break
    if (value_found == False):
      csv_keyframe_numbers.append(rot_array_cp[0][i])