def u_P_lcs2gcs(uP_lcs, q, ID):
"""
Function evaluates coordinates of a point uP in GCS based on coordinates in LCS
:param uP_lcs: a point vector in LCS 2D (x, y) (type: numpy array)
:param q: a vector of system coordinates and velocities (type:numpy array)
:param id: a id of a body (int)
:return rP_gcs: a vector of point in GCS
"""
rP_gcs = q2R_i(q, ID) + Ai_ui_P_vector(uP_lcs, q2theta_i(q, ID))
return rP_gcs
def u_P_gcs2lcs(rP_gcs, q, id):
"""
Function evaluates coordinates of a point uP in LCS based on coordinates in GCS
:param uP_lcs: a point vector in LCS 2D (x, y) (type: numpy array)
:param q: a vector of system coordinates and velocities (type:numpy array)
:param id: a id of a body (int)
:return:
"""
uP_gcs = np.dot(A_inv_matrix(q2theta_i(q, id)), rP_gcs - q2R_i(q, id))
return uP_gcs
def u_P_lcs2gcs(uP_lcs, q, body_id, node_id=None):
"""
Function evaluates coordinates of a point uP in GCS based on coordinates in LCS
:param uP_lcs: a point vector in LCS 2D (x, y) (type: numpy array)
:param q: a vector of system coordinates and velocities (type:numpy array)
:param id: a id of a body (int)
:return rP_gcs: a vector of point in GCS
"""
rP_gcs = q2R_i(q, body_id) + Ai_ui_P_vector(
uP_lcs, q2theta_i(q, body_id, node_id=node_id))
return rP_gcs
def q2q_body(q, body_id):
"""
Function returns the velocity vector of a body from vector q and body_id.
Args:
q - vector of positions and velocities
body_id - body_id generated when creating body object
N_b - number of bodies
Returns:
dR_i_ - velocity vector of translational and angular velocity of a body with id body_id
Raises:
None
"""
q_body = np.zeros(3)
q_body[0:2] = q2R_i(q, body_id)
q_body[2] = q2theta_i(q, body_id)
return q_body
