113 lines
2.9 KiB
Python
Executable File
113 lines
2.9 KiB
Python
Executable File
import math
|
|
import numpy as np
|
|
import copy
|
|
|
|
def coordinate_transformation_in_angle(positions, base_angle):
|
|
'''
|
|
Transformation the coordinate in the angle
|
|
|
|
Parameters
|
|
-------
|
|
positions : numpy.ndarray
|
|
this parameter is composed of xs, ys
|
|
should have (2, N) shape
|
|
base_angle : float [rad]
|
|
|
|
Returns
|
|
-------
|
|
traslated_positions : numpy.ndarray
|
|
the shape is (2, N)
|
|
|
|
'''
|
|
if positions.shape[0] != 2:
|
|
raise ValueError('the input data should have (2, N)')
|
|
|
|
positions = np.array(positions)
|
|
positions = positions.reshape(2, -1)
|
|
|
|
rot_matrix = [[np.cos(base_angle), np.sin(base_angle)],
|
|
[-1*np.sin(base_angle), np.cos(base_angle)]]
|
|
|
|
rot_matrix = np.array(rot_matrix)
|
|
|
|
translated_positions = np.dot(rot_matrix, positions)
|
|
|
|
return translated_positions
|
|
|
|
def coordinate_transformation_in_position(positions, base_positions):
|
|
'''
|
|
Transformation the coordinate in the positions
|
|
|
|
Parameters
|
|
-------
|
|
positions : numpy.ndarray
|
|
this parameter is composed of xs, ys
|
|
should have (2, N) shape
|
|
base_positions : numpy.ndarray
|
|
this parameter is composed of x, y
|
|
shoulg have (2, 1) shape
|
|
|
|
Returns
|
|
-------
|
|
traslated_positions : numpy.ndarray
|
|
|
|
'''
|
|
|
|
if positions.shape[0] != 2:
|
|
raise ValueError('the input data should have (2, N)')
|
|
|
|
positions = np.array(positions)
|
|
positions = positions.reshape(2, -1)
|
|
base_positions = np.array(base_positions)
|
|
base_positions = base_positions.reshape(2, 1)
|
|
|
|
translated_positions = positions - base_positions
|
|
|
|
return translated_positions
|
|
|
|
|
|
def coordinate_transformation_in_matrix_angles(positions, base_angles):
|
|
'''
|
|
Transformation the coordinate in the matrix angle
|
|
|
|
Parameters
|
|
-------
|
|
positions : numpy.ndarray
|
|
this parameter is composed of xs, ys
|
|
should have (2, N) shape
|
|
base_angle : float [rad]
|
|
|
|
Returns
|
|
-------
|
|
traslated_positions : numpy.ndarray
|
|
the shape is (2, N)
|
|
|
|
'''
|
|
if positions.shape[0] != 2:
|
|
raise ValueError('the input data should have (2, N)')
|
|
|
|
positions = np.array(positions)
|
|
positions = positions.reshape(2, -1)
|
|
translated_positions = np.zeros_like(positions)
|
|
|
|
for i in range(len(base_angles)):
|
|
rot_matrix = [[np.cos(base_angles[i]), np.sin(base_angles[i])],
|
|
[-1*np.sin(base_angles[i]), np.cos(base_angles[i])]]
|
|
|
|
rot_matrix = np.array(rot_matrix)
|
|
|
|
translated_position = np.dot(rot_matrix, positions[:, i].reshape(2, 1))
|
|
|
|
translated_positions[:, i] = translated_position.flatten()
|
|
|
|
return translated_positions.reshape(2, -1)
|
|
|
|
# def coordinate_inv_transformation
|
|
if __name__ == '__main__':
|
|
positions_1 = np.array([[1.0], [2.0]])
|
|
base_angle = 1.25
|
|
|
|
translated_positions_1 = coordinate_transformation_in_angle(positions_1, base_angle)
|
|
print(translated_positions_1)
|
|
|