######################################################
#
#NAME
# GravityBoomConfigurationForSatelliteControl.py
#
#PURPOSE:
# This is just a setup file to configure the 
# angular velocity and moment of inertia tensor to 
# simulate a gravity stabilized satellite for the 
# SatelliteControl.py script.  See that file for more 
# details.
#
######################################################
RotateYawBy5Degrees=0

#Set up the moment of inertia tensor to be concentrated along the z axis
cxbn.InertiaTensor[0,0] = 10.000 
cxbn.InertiaTensor[0,1] = 0.000
cxbn.InertiaTensor[0,2] = 0.000
cxbn.InertiaTensor[1,0] = 0.000
cxbn.InertiaTensor[1,1] = 10.000 
cxbn.InertiaTensor[1,2] = 0.000
cxbn.InertiaTensor[2,0] = 0.000
cxbn.InertiaTensor[2,1] = 0.000
cxbn.InertiaTensor[2,2] = 0.100

#Artificially set the omega's to zero
omega.x=0 ; omega.y=0 ; omega.z=0
omega_t0=omega ; omega_t1=omega

#Set up the basis vectors for the inertial frame
i   = vec3(1,0,0) ; j   = vec3(0,1,0)    ; k   = vec3(0,0,1)
e_i = vec3(1,0,0) ; e_j = vec3(0,1,0)    ; e_k = vec3(0,0,1)
r_i = v_t_norm    ; r_j = r_v_cross_norm ; r_k = r_eci_norm
b_i = v_t_norm    ; b_j = r_v_cross_norm ; b_k = r_eci_norm

#Allow for a rotation about the pitch axis to match up with the results from 
#Sidi pg. 117.  See Analytical Mechanics...page 91 for details
if RotateYawBy5Degrees == 1:

  #Set up the same inertia matrix
  cxbn.InertiaTensor[0,0] = 80.000
  cxbn.InertiaTensor[0,1] = 0.000
  cxbn.InertiaTensor[0,2] = 0.000
  cxbn.InertiaTensor[1,0] = 0.000
  cxbn.InertiaTensor[1,1] = 82.000
  cxbn.InertiaTensor[1,2] = 0.000
  cxbn.InertiaTensor[2,0] = 0.000
  cxbn.InertiaTensor[2,1] = 0.000
  cxbn.InertiaTensor[2,2] = 4.000

  #Get the rotation matrix for the principal axis along b_i
  Yaw = -5*pi/180.

  #Notational shorthands
  p = b_k
  C = math.cos(Yaw)
  S = math.sin(Yaw)
  Sig = 1-C

  #Form the matrix
  YawDCM = mat3()
  YawDCM[0,0] = p.x**2*Sig+C 
  YawDCM[0,1] = p.x*p.y*Sig+p.z*S 
  YawDCM[0,2] = p.x*p.z*Sig-p.y*S
  YawDCM[1,0] = p.y*p.x*Sig-p.z*S
  YawDCM[1,1] = p.y**2*Sig+C
  YawDCM[1,2] = p.y*p.z*Sig+p.x*S
  YawDCM[2,0] = p.z*p.x*Sig+p.y*S
  YawDCM[2,1] = p.z*p.y*Sig-p.x*S
  YawDCM[2,2] = p.z**2*Sig+C
  
  #Now rotate the basis vectors of the body frame 
  b_i = YawDCM*b_i
  b_j = YawDCM*b_j
  b_k = YawDCM*b_k

#Get the initial Direction Cosine Matrices
e_2_b=ReturnDCM(e_i, e_j, e_k, b_i, b_j, b_k)
r_2_b=ReturnDCM(r_i, r_j, r_k, b_i, b_j, b_k)

#Get the initial quaternion
quat_initial = quat(e_2_b)                #Get the DCM from the quaternion
quat_initial = quat_initial.normalize()   #normalize the quaternion
q_t0 = vec4()                             #quaternion at time step [t]
q_t1 = vec4()                             #quaternion at time step [t+1]
q_t0[3] = quat_initial.w
q_t0[0] = quat_initial.x
q_t0[1] = quat_initial.y
q_t0[2] = quat_initial.z
q_t1 = q_t0

#Use only the gravity gradient torque
UseGGTorque = 1
UseAeroTorque = 0
