"""
author: Faisal Z. Qureshi
email: faisal.qureshi@ontariotechu.ca
website: http://www.vclab.ca
license: BSD
"""


import numpy as np
from scipy.spatial.transform import Rotation as R

#     H ------ G
#    /|       /|
#   / E ---- / F
#  / /      / /
# D ------ C /
# |/       |/
# A ------ B

m = np.ones(8)
r = np.empty((3,8))

l = 4
h = 1
d = 2 

r[:,0] = np.array([1,1,1])            # A
r[:,1] = r[:,0] + np.array([l,0,0])   # B
r[:,2] = r[:,0] + np.array([l,h,0])   # C
r[:,3] = r[:,0] + np.array([0,h,0])   # D
r[:,4] = r[:,0] + np.array([0,0,-d])  # E
r[:,5] = r[:,4] + np.array([l,0,0])   # F
r[:,6] = r[:,4] + np.array([l,h,0])   # G
r[:,7] = r[:,4] + np.array([0,h,0])   # H

print('--------------------------------------------------')
print('                   UNROTATED BODY')
print('--------------------------------------------------')

print('m:\n', m)
print('r:\n', r)

M = np.sum(m) # Total mass
print('M:\n', M)

center_of_mass = np.sum(r * np.tile(m, (3,1)), axis=1) / M
print('center of mass:\n', center_of_mass)

rp = r - np.tile(center_of_mass , (8,1)).T 
print('rp:\n', rp) 

I = np.empty((3,3))
I[0,0] = np.sum(np.multiply(np.power(rp[1,:],2) + np.power(rp[2,:],2), m))
I[1,1] = np.sum(np.multiply(np.power(rp[2,:],2) + np.power(rp[0,:],2), m))
I[2,2] = np.sum(np.multiply(np.power(rp[0,:],2) + np.power(rp[1,:],2), m))
I[0,1] = I[1,0] = np.sum(np.multiply(np.multiply(rp[0,:], rp[1,:]), m))
I[0,2] = I[2,0] = np.sum(np.multiply(np.multiply(rp[0,:], rp[2,:]), m))
I[1,2] = I[2,1] = np.sum(np.multiply(np.multiply(rp[1,:], rp[2,:]), m))
print('I:\n', I)

print('--------------------------------------------------')
print('                ROTATED BODY')
print('--------------------------------------------------')

rot_mat = R.from_euler('y',45, degrees=True).as_matrix()
print('rot_mat:\n', rot_mat)

# Note:  1) rp; 2) center of mass; and 3) overwriting r
r = np.dot(rot_mat, rp) + np.tile(center_of_mass, (8,1)).T 
print('rotated_r:\n', r)

center_of_mass = np.sum(r * np.tile(m, (3,1)), axis=1) / M
print('center of mass:\n', center_of_mass)

rp = r - np.tile(center_of_mass , (8,1)).T 
print('rp:\n', rp) 

I = np.empty((3,3))
I[0,0] = np.sum(np.multiply(np.power(rp[1,:],2) + np.power(rp[2,:],2), m))
I[1,1] = np.sum(np.multiply(np.power(rp[2,:],2) + np.power(rp[0,:],2), m))
I[2,2] = np.sum(np.multiply(np.power(rp[0,:],2) + np.power(rp[1,:],2), m))
I[0,1] = I[1,0] = np.sum(np.multiply(np.multiply(rp[0,:], rp[1,:]), m))
I[0,2] = I[2,0] = np.sum(np.multiply(np.multiply(rp[0,:], rp[2,:]), m))
I[1,2] = I[2,1] = np.sum(np.multiply(np.multiply(rp[1,:], rp[2,:]), m))
print('I:\n', I)

# Computing I_body using rotation matrix
I_body = np.dot(np.dot(rot_mat.T, I), rot_mat) 
print('I_body:\n', I_body)

# Computing I_body using eigenvalues and eigenvectors
w, v = np.linalg.eig(I)
print('eigenvalues:\n', w)
print('eigenvectors:\n', v)