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


import numpy as np

#     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('m:\n', m)
print('r:\n', r)

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

m_tmp = np.tile(m, (3,1))
print(r * np.tile(m, (3,1)))

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