def __init__(self, frame=0):
self.name = ''
self.format_name = ''
self.frame = frame
self.position = QVector3D(0, 0, 0)
self.rotation = QQuaternion()
self.org_position = QVector3D(0, 0, 0)
self.org_rotation = QQuaternion()
self.complement = [
20, 20, 0, 0, 20, 20, 20, 20, 107, 107, 107, 107, 107, 107, 107,
107, 20, 20, 20, 20, 20, 20, 20, 107, 107, 107, 107, 107, 107, 107,
107, 0, 20, 20, 20, 20, 20, 20, 107, 107, 107, 107, 107, 107, 107,
107, 0, 0, 20, 20, 20, 20, 20, 107, 107, 107, 107, 107, 107, 107,
107, 0, 0, 0
]
self.org_complement = [
20, 20, 0, 0, 20, 20, 20, 20, 107, 107, 107, 107, 107, 107, 107,
107, 20, 20, 20, 20, 20, 20, 20, 107, 107, 107, 107, 107, 107, 107,
107, 0, 20, 20, 20, 20, 20, 20, 107, 107, 107, 107, 107, 107, 107,
107, 0, 0, 20, 20, 20, 20, 20, 107, 107, 107, 107, 107, 107, 107,
107, 0, 0, 0
]
# 登録対象であるか否か
self.key = False
# VMD読み込み処理で読み込んだキーか
self.read = False
# 補間曲線の分割で追加したキーか
self.split_complement = False
def move(self, x, y, width, height):
self.currentPos = self.mapToSphere(x, y, width, height)
self._axis = QVector3D.crossProduct(self.lastPos, self.currentPos)
length = math.sqrt(QVector3D.dotProduct(self.axis, self.axis))
self.angle = QVector3D.dotProduct(self.lastPos, self.currentPos)
self.lastPos = self.currentPos
if length > EPSILON:
return QQuaternion.fromAxisAndAngle(self._axis, self.angle)
return QQuaternion.fromAxisAndAngle(0, 0, 0, 0)
def _bucketQuaternion(self):
if 'bucket_orientation' in self.stateObject.getState()['quaternions']:
components = self.stateObject.getState(
)['quaternions']['bucket_orientation']
quart = QQuaternion(components['w'], components['x'],
components['y'],
components['z']).toEulerAngles()
return quart.x()
else:
return 0
def __init__(self, parent=None):
super().__init__(
get_resources_path(
os.path.join('plugin_system', 'plugins', 'imu_v2',
'imu_v2.obj')), parent)
self.save_orientation_flag = False
self.has_save_orientation = False
self.reference_orientation = QQuaternion()
self.rotation = QQuaternion()
def update_orientation(self, w, x, y, z):
if self.save_orientation_flag:
self.reference_orientation = QQuaternion(w, x, y, z)
self.save_orientation_flag = False
self.has_save_orientation = True
if not self.has_save_orientation:
return
self.rotation = self.reference_orientation.conjugate() * QQuaternion(
w, x, y, z)
super().update()
def calc_ankle_rotation(from_pos, to_pos):
from_qq = QQuaternion()
if from_pos != QVector3D() and to_pos != QVector3D():
to_pos = to_pos - from_pos
to_pos.normalize()
logger.debug("to_pos: %s", to_pos)
# 水平からTOボーンまでの回転量
from_qq = QQuaternion.rotationTo(QVector3D(to_pos.x(), 1, -1), to_pos)
logger.debug("from_pos: %s, to_pos: %s, from_qq: %s", from_pos, to_pos,
from_qq.toEulerAngles())
return from_qq
def __init__(self, frame=0):
self.name = ''
self.format_name = ''
self.frame = frame
self.position = QVector3D(0, 0, 0)
self.rotation = QQuaternion()
self.complement = [
20, 20, 0, 0, 20, 20, 20, 20, 107, 107, 107, 107, 107, 107, 107,
107, 20, 20, 20, 20, 20, 20, 20, 107, 107, 107, 107, 107, 107, 107,
107, 0, 20, 20, 20, 20, 20, 20, 107, 107, 107, 107, 107, 107, 107,
107, 0, 0, 20, 20, 20, 20, 20, 107, 107, 107, 107, 107, 107, 107,
107, 0, 0, 0
]
self.key = True
def _bucket(self):
if self.useQuarternions and 'bucket_orientation' in self.stateObject.getState(
)['quaternions']:
components = self.stateObject.getState(
)['quaternions']['bucket_orientation']
quart = QQuaternion(components['w'], components['x'],
components['y'],
components['z']).toEulerAngles()
return quart.x()
elif not self.useQuarternions and 'bucket' in self.stateObject.getState(
)['angles']:
return self.stateObject.getState()['angles']['bucket'] / 10
else:
return -90
def _digging_arm(self):
if self.useQuarternions and 'digging_arm_orientatation' in self.stateObject.getState(
)['quaternions']:
components = self.stateObject.getState(
)['quaternions']['digging_arm_orientation']
quart = QQuaternion(components['w'], components['x'],
components['y'],
components['z']).toEulerAngles()
return quart.x()
elif not self.useQuarternions and 'digging_arm' in self.stateObject.getState(
)['angles']:
return self.stateObject.getState()['angles']['digging_arm'] / 10
else:
return -100
def calc_global_camera_pos(cf):
camera_pos = cf.position
# カメラの角度
camera_qq = QQuaternion.fromEulerAngles(degrees(cf.euler.x()),
degrees(cf.euler.y()),
degrees(cf.euler.z()))
logger.info("cf.euler: %s", cf.euler)
logger.info("camera_qq: %s", camera_qq.toEulerAngles())
logger.info("cf.position: %s", cf.position)
logger.info("cf.length: %s", cf.length)
mat = QMatrix4x4()
# カメラの回転
mat.rotate(camera_qq)
# 初期位置
mat.translate(cf.position)
camera_pos = mat * QVector3D()
# # とりあえずZ距離はなし
# camera_pos.setZ(0)
# if cf.position.y() < 0 and camera_pos.y() > 0:
# # 元々のY位置がマイナスで、計算後符号が反転した場合、減算
# camera_pos.setY( cf.position.y() - camera_pos.y() )
logger.info("camera_pos cf.position: %s", cf.position)
logger.info("camera_pos mat: %s", camera_pos)
return camera_pos
def createScene():
# Root entity.
rootEntity = QEntity()
# Material.
material = QPhongMaterial(rootEntity)
# Torus.
torusEntity = QEntity(rootEntity)
torusMesh = QTorusMesh()
torusMesh.setRadius(5)
torusMesh.setMinorRadius(1)
torusMesh.setRings(100)
torusMesh.setSlices(20)
torusTransform = QTransform()
torusTransform.setScale3D(QVector3D(1.5, 1.0, 0.5))
torusTransform.setRotation(
QQuaternion.fromAxisAndAngle(QVector3D(1.0, 0.0, 0.0), 45.0))
torusEntity.addComponent(torusMesh)
torusEntity.addComponent(torusTransform)
torusEntity.addComponent(material)
# Sphere.
sphereEntity = QEntity(rootEntity)
sphereMesh = QSphereMesh()
sphereMesh.setRadius(3)
sphereEntity.addComponent(sphereMesh)
sphereEntity.addComponent(material)
return rootEntity
class IMUV23DWidget(RenderWidget):
def __init__(self, parent=None):
super().__init__(get_resources_path(os.path.join('plugin_system', 'plugins', 'imu_v2', 'imu_v2.obj')), parent)
self.save_orientation_flag = False
self.has_save_orientation = False
self.reference_orientation = QQuaternion()
self.rotation = QQuaternion()
def save_orientation(self):
self.save_orientation_flag = True
def get_state(self):
return self.save_orientation_flag, self.reference_orientation, self.has_save_orientation
def set_state(self, tup):
self.save_orientation_flag, self.reference_orientation, self.has_save_orientation = tup
def get_model_matrix(self):
result = super().get_model_matrix()
result.rotate(self.rotation)
return result
def update_orientation(self, w, x, y, z):
if self.save_orientation_flag:
self.reference_orientation = QQuaternion(w, x, y, z)
self.save_orientation_flag = False
self.has_save_orientation = True
if not self.has_save_orientation:
return
self.rotation = self.reference_orientation.conjugate() * QQuaternion(w, x, y, z)
super().update()
def reset(self, quat=QQuaternion()):
"""Reset trackball"""
self._rotation = quat
self._angularVelocity = 0.0
self._lastPos = QPointF()
self._lastTime = QTime.currentTime()
self._pressed = False
self._paused = False
def smooth_angle_bone(bone_frame_dic, smooth_times, target_bones):
# 関節の角度円滑化
for bone_name in target_bones:
for n in range(smooth_times):
for frame in range(len(bone_frame_dic[bone_name])):
if frame >= 2:
prev2_bf = bone_frame_dic[bone_name][frame - 2]
prev1_bf = bone_frame_dic[bone_name][frame - 1]
now_bf = bone_frame_dic[bone_name][frame]
if prev2_bf != now_bf.rotation:
# 角度が違っていたら、球形補正開始
euler = QQuaternion.slerp(prev2_bf.rotation, now_bf.rotation, 0.5).toEulerAngles()
euler.setX(0 if np.isnan(euler.x()) else euler.x())
euler.setY(0 if np.isnan(euler.x()) else euler.y())
euler.setZ(0 if np.isnan(euler.x()) else euler.z())
prev1_bf.rotation = QQuaternion.fromEulerAngles(euler)
def __init__(self, parent=None):
super().__init__(get_resources_path(os.path.join('plugin_system', 'plugins', 'imu_v2', 'imu_v2.obj')), parent)
self.save_orientation_flag = False
self.has_save_orientation = False
self.reference_orientation = QQuaternion()
self.rotation = QQuaternion()
def reduce_bone_frame(v, head, tail, threshold_pos, threshold_rot):
# 移動のエラー最大値
max_pos_err = float(0.0)
# 回転のエラー最大値
max_rot_err = float(0.0)
# 移動：エラー最大値のindex
max_idx_pos = 0
# 回転：エラー最大値のindex
max_idx_rot = 0
# 最初から最後までのフレーム数
total = tail - head
for i in range(head + 1, tail , 1):
# 移動
ip_pos = v[head].position + (v[tail].position - v[head].position) * (i - head) / total
pos_err = (ip_pos - v[i].position).length()
if pos_err > max_pos_err:
max_idx_pos = i
max_pos_err = pos_err
t = float(i - head) / total
# 回転
ip_rot = QQuaternion.slerp(v[head].rotation, v[tail].rotation, t)
q_err = (ip_rot * v[i].rotation.inverted()).normalized()
# フィルタではなく、ここで正負反転させてプラスに寄せる
if q_err.scalar() < 0:
q_err.setX(q_err.x() * -1)
q_err.setY(q_err.y() * -1)
q_err.setX(q_err.z() * -1)
q_err.setScalar(q_err.scalar() * -1)
# math.acos(q_err.scalar()) * 2 * 180 / math.pi
rot_err = math.degrees(math.acos(q_err.scalar()))
# logger.info("rot_err: %s, %s", rot_err, max_rot_err)
if rot_err > max_rot_err:
max_idx_rot = i
max_rot_err = rot_err
v1 = []
if max_pos_err > threshold_pos:
v1 = reduce_bone_frame(v, head, max_idx_pos, threshold_pos, threshold_rot)
v2 = reduce_bone_frame(v, max_idx_pos, tail, threshold_pos, threshold_rot)
v1.extend(v2)
else:
if max_rot_err > threshold_rot:
v1 = reduce_bone_frame(v, head, max_idx_rot, threshold_pos, threshold_rot)
v2 = reduce_bone_frame(v, max_idx_rot, tail, threshold_pos, threshold_rot)
v1.extend(v2)
else:
v1.append(v[head])
return v1
def generateData(self):
# 生成模拟数据
magneticFieldArray = []
for i in range(self.m_fieldLines):
horizontalAngle = (self.doublePi * i) / self.m_fieldLines
xCenter = self.ellipse_a * math.cos(horizontalAngle)
zCenter = self.ellipse_a * math.sin(horizontalAngle)
# Rotate - arrow is always tangential to the origin.
# 旋转-箭头始终与原点相切。
yRotation = QQuaternion.fromAxisAndAngle(
0.0, 1.0, 0.0, horizontalAngle * self.radiansToDegrees)
for j in range(self.m_arrowsPerLine):
# Calculate the point on the ellipse centered on the origin and
# 计算椭圆上以原点为中心的点
# parallel to the x-axis.
# 平行于X轴。
verticalAngle = ((self.doublePi * j) /
self.m_arrowsPerLine) + self.m_angleOffset
xUnrotated = self.ellipse_a * math.cos(verticalAngle)
y = self.ellipse_b * math.sin(verticalAngle)
# Rotate the ellipse around the y-axis.
# 围绕Y轴旋转椭圆。
xRotated = xUnrotated * math.cos(horizontalAngle)
zRotated = xUnrotated * math.sin(horizontalAngle)
# Add the offset.
# 添加偏移量。
x = xCenter + xRotated
z = zCenter + zRotated
zRotation = QQuaternion.fromAxisAndAngle(
0.0, 0.0, 1.0, verticalAngle * self.radiansToDegrees)
totalRotation = yRotation * zRotation
itm = QScatterDataItem(QVector3D(x, y, z), totalRotation)
magneticFieldArray.append(itm)
if self.m_graph.selectedSeries() is self.m_magneticField:
self.m_graph.clearSelection()
self.m_magneticField.dataProxy().resetArray(magneticFieldArray)
def mouseMoveEvent(self, event):
"""
Called by the Qt libraries whenever the window receives a mouse
move/drag event.
"""
btns = event.buttons()
# pixel coordinates relative to the window
x = event.localPos().x()
y = event.localPos().y()
if btns & Qt.LeftButton:
# Rotation via emulated trackball using quaternions.
# For method employed see:
# https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
# get current vector from sphere/trackball center to surface
mouse_rotation_current_vec = self._find_trackball_vector(x, y)
# get the angle between the vector which was stored at the
# time of mouse click and the current vector
angle_between = PainterWidget.angle_between(
mouse_rotation_current_vec,
self._mouse_rotation_start_vec)
angle_between *= 20 # arbitrary amplification for faster rotation
# get the rotation axis which is perpendicular to both vectors
rotation_axis = QVector3D.crossProduct(
self._mouse_rotation_start_vec,
mouse_rotation_current_vec)
# create a rotated normalized quaternion corresponding to the
# drag distance travelled by the mouse since the click
delta = QQuaternion.fromAxisAndAngle(rotation_axis, angle_between)
delta.normalize()
# rotate self._rotation_quat (used to rotate the View matrix)
self._rotation_quat = delta * self._rotation_quat_start
self._rotation_quat.normalize()
elif btns & Qt.MidButton:
# Translation left/right and up/down depending on camera orientation
diff = QVector3D(x, y, 0) - self._mouse_translation_start_vec
diff_x = diff[0]
diff_y = diff[1]
self._translation_vec = self._translation_vec_start - self.cam_right * diff_x * 2 + self.cam_up * diff_y * 2
elif btns & Qt.RightButton:
# Translation forward/backward depending on camera orientation
diff_y = y - self._mouse_camforward_start
self._translation_vec = self._translation_vec_start - self.cam_look * diff_y * 2
# re-draw at next timer tick
self.dirty = True
def rotation(self):
"""Returns rotation quarternion"""
if self._paused or self._pressed:
return self._rotation
currentTime = QTime.currentTime()
angle = self._angularVelocity * self._lastTime.msecsTo(currentTime)
return QQuaternion.fromAxisAndAngle(self._axis, angle) * self._rotation
def move(self, point, quat):
"""Move trackball"""
if not self._pressed:
return
currentTime = QTime.currentTime()
msecs = self._lastTime.msecsTo(currentTime)
if msecs <= 20:
return
if self._mode == self.TrackballMode.Planar:
delta = QLineF(self._lastPos, point)
self._angularVelocity = 180.0 * delta.length() / (math.pi * msecs)
self._axis = QVector3D(-delta.dy(), delta.dx(), 0.0).normalized()
self._axis = quat.rotatedVector(self._axis)
self._rotation = QQuaternion.fromAxisAndAngle(self._axis, 180.0 / math.pi * delta.length()) * self._rotation
elif self._mode == self.TrackballMode.Spherical:
lastPos3D = QVector3D(self._lastPos.x(), self._lastPos.y(), 0.0)
sqrZ = 1.0 - QVector3D.dotProduct(lastPos3D, lastPos3D)
if sqrZ > 0:
lastPos3D.setZ(math.sqrt(sqrZ))
else:
lastPos3D.normalize()
currentPos3D = QVector3D(point.x(), point.y(), 0.0)
sqrZ = 1.0 - QVector3D.dotProduct(currentPos3D, currentPos3D)
if sqrZ > 0:
currentPos3D.setZ(math.sqrt(sqrZ))
else:
currentPos3D.normalize()
self._axis = QVector3D.crossProduct(lastPos3D, currentPos3D)
angle = 180.0 / math.pi * math.asin(math.sqrt(QVector3D.dotProduct(self._axis, self._axis)))
self._angularVelocity = angle / msecs
self._axis.normalize()
self._axis = quat.rotatedVector(self._axis)
self._rotation = QQuaternion.fromAxisAndAngle(self._axis, angle) * self._rotation
self._lastPos = point
self._lastTime = currentTime
def Updategui(self, data): # sets and processes gui data
if self.data['Speed']['Abs']!=[]:
self.ui.sabsval.setText(str(self.data['Speed']['Abs'][-1]))
if self.data['Speed']['Vertical'] !=[]:
self.ui.sverval.setText(str(self.data['Speed']['Vertical'][-1]))
if self.data['Speed']['Horizontal'] !=[]:
self.ui.shorval.setText(str(self.data['Speed']['Horizontal'][-1]))
if self.data['Speed']['Angular'] !=[]:
self.ui.sangval.setText(str(self.data['Speed']['Angular'][-1]))
if self.data['Speed']['Top'] !=[]:
self.ui.stopval.setText(str(self.data['Speed']['Top'][-1]))
if self.data['Acceleration']['Abs'] !=[]:
self.ui.aabsval.setText(str(self.data['Acceleration']['Abs'][-1]))
if self.data['Acceleration']['Vertical'] !=[]:
self.ui.averval.setText(str(self.data['Acceleration']['Vertical'][-1]))
if self.data['Acceleration']['Horizontal'] !=[]:
self.ui.ahorval.setText(str(self.data['Acceleration']['Horizontal'][-1]))
if self.data['Acceleration']['Angular'] !=[]:
self.ui.aangval.setText(str(self.data['Acceleration']['Angular'][-1]))
if self.data['Acceleration']['Top'] !=[]:
self.ui.atopval.setText(str(self.data['Acceleration']['Top'][-1]))
if self.data['Position']['Abs'] !=[]:
self.ui.pabsval.setText(str(self.data['Position']['Abs'][-1]))
if self.data['Position']['Altitude'] !=[]:
self.ui.paltval.setText(str(self.data['Position']['Altitude'][-1]))
if self.data['Position']['Horizontal'] !=[]:
self.ui.phorval.setText(str(self.data['Position']['Horizontal'][-1]))
if self.data['Position']['Latitude'] !=[]:
self.ui.latitudeval.setText(str(self.data['Position']['Latitude'][-1]))
if self.data['Position']['Longitude'] !=[]:
self.ui.longval.setText(str(self.data['Position']['Longitude'][-1]))
if self.data['Orientation']['Roll'] !=[]:
self.ui.orolval.setText(str(self.data['Orientation']['Roll'][-1]))
if self.data['Orientation']['Pitch'] !=[]:
self.ui.opitval.setText(str(self.data['Orientation']['Pitch'][-1]))
if self.data['Orientation']['Yaw'] !=[]:
self.ui.oyawval.setText(str(self.data['Orientation']['Yaw'][-1]))
try:
self.plot.pw.setTitle('T +' + str(data[0]))
except:
pass
try:
self.gyro.rocketTransform.setRotation(QQuaternion.fromEulerAngles(data[0], data[1], data[2]))
except:
pass
for key in list(self.listofplots.keys()):
key = key.split('.')
self.listofplots[key[0] + '.' + key[1]].setData(x=self.time['Time'], y=self.data[key[0]][key[1]],
clear=True)
def __init__(self, parent, refresh_rate = 20):
super(PainterWidget, self).__init__(parent)
self.mat_v = QMatrix4x4() # the current View matrix
self.mat_v_inverted = QMatrix4x4() # the current inverse View matrix
self.mat_p = QMatrix4x4() # the current Projection matrix
self.cam_right = QVector3D() # the current camera right direction
self.cam_up = QVector3D() # the current camera up direction
self.cam_look = QVector3D() # the current camera look direction
self.cam_pos = QVector3D() # the current camera position
self.fov = 90 # the current field of view for the projection matrix
# The width and height of the window. resizeGL() will set them.
self.width = None
self.height = None
# Rather than repainting the scene on each mouse event,
# we repaint at fixed timer intervals.
self.dirty = True
# Setup our only and main timer
self._timer = QTimer()
self._timer.timeout.connect(self._timer_timeout)
# contains OpenGL "programs" of different shaders
self.programs = {}
# Setup inital world Rotation states
self._rotation_quat = QQuaternion() # to rotate the View matrix
self._rotation_quat_start = None # state for mouse click
self._mouse_rotation_start_vec = None # state for mouse click
# Setup initial world Translation states
self._translation_vec = QVector3D(-150, -150, -350) # to translate the View matrix
self._translation_vec_start = None # state for mouse click
self._mouse_fov_start = None # state for mouse click
self._refresh_rate = refresh_rate
def update_orientation(self, w, x, y, z):
if self.save_orientation_flag:
self.reference_orientation = QQuaternion(w, x, y, z)
self.save_orientation_flag = False
self.has_save_orientation = True
if not self.has_save_orientation:
return
self.rotation = self.reference_orientation.conjugate() * QQuaternion(w, x, y, z)
super().update()
def position_to_frame_leg_one_side_calc(frame, pos, lower_correctqq,
lower_body_rotation, points,
slope_motion, direction_name):
# 足
direction = pos[points['Knee']] - pos[points['Hip']]
up = QVector3D.crossProduct((pos[points['Knee']] - pos[points['Hip']]),
(pos[points['Foot']] - pos[points['Knee']]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(-1, 0, 0))
rotation = lower_correctqq * orientation * initial_orientation.inverted()
# 足の傾きデータ
leg_correctqq = QQuaternion()
if slope_motion is not None:
# Y軸の回転具合を求める
y_degree = (180 - abs(rotation.toEulerAngles().y())) / 180
# 一旦オイラー角に変換して、角度のかかり具合を補正し、再度クォータニオンに変換する
leg_correctqq = QQuaternion.fromEulerAngles(slope_motion.frames[
"{0}足".format(direction_name)][0].rotation.toEulerAngles() *
y_degree).inverted()
leg_rotation = leg_correctqq * lower_body_rotation.inverted() * rotation
# ひざ
bf = VmdBoneFrame(frame)
bf.name = b'\x8d\xb6\x82\xd0\x82\xb4' # 'ひざ'
direction = pos[points['Foot']] - pos[points['Knee']]
up = QVector3D.crossProduct((pos[points['Knee']] - pos[points['Hip']]),
(pos[points['Foot']] - pos[points['Knee']]))
orientation = QQuaternion.fromDirection(direction, up)
initial_orientation = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(-1, 0, 0))
rotation = lower_correctqq * orientation * initial_orientation.inverted()
# ひざの傾きデータ
knee_correctqq = QQuaternion()
if slope_motion is not None:
# Y軸の回転具合を求める
y_degree = (180 - abs(rotation.toEulerAngles().y())) / 180
# 一旦オイラー角に変換して、角度のかかり具合を補正し、再度クォータニオンに変換する
knee_correctqq = QQuaternion.fromEulerAngles(slope_motion.frames[
"{0}ひざ".format(direction_name)][0].rotation.toEulerAngles() *
y_degree).inverted()
knee_rotation = knee_correctqq * leg_rotation.inverted(
) * lower_body_rotation.inverted() * rotation
return leg_rotation, knee_rotation
def createScene():
# Root entity
rootEntity = QEntity()
# Material
material = QPhongMaterial(rootEntity)
# Torus
torusEntity = QEntity(rootEntity)
# Qt3DExtras.QTorusMesh *
torusMesh = QTorusMesh()
torusMesh.setRadius(5)
torusMesh.setMinorRadius(1)
torusMesh.setRings(100)
torusMesh.setSlices(20)
# Qt3DCore.QTransform *
torusTransform = QTransform()
torusTransform.setScale3D(QVector3D(1.5, 1, 0.5))
torusTransform.setRotation(
QQuaternion.fromAxisAndAngle(QVector3D(1, 0, 0), 45.0))
torusEntity.addComponent(torusMesh)
torusEntity.addComponent(torusTransform)
torusEntity.addComponent(material)
# Sphere
sphereEntity = QEntity(rootEntity)
sphereMesh = QSphereMesh()
sphereMesh.setRadius(3)
# Qt3DCore.QTransform *
sphereTransform = QTransform()
# OrbitTransformController *
controller = OrbitTransformController(sphereTransform)
controller.setTarget(sphereTransform)
controller.setRadius(20.0)
# QPropertyAnimation *
sphereRotateTransformAnimation = QPropertyAnimation(sphereTransform)
sphereRotateTransformAnimation.setTargetObject(controller)
sphereRotateTransformAnimation.setPropertyName(b"angle")
sphereRotateTransformAnimation.setStartValue(0)
sphereRotateTransformAnimation.setEndValue(360)
sphereRotateTransformAnimation.setDuration(10000)
sphereRotateTransformAnimation.setLoopCount(-1)
sphereRotateTransformAnimation.start()
sphereEntity.addComponent(sphereMesh)
sphereEntity.addComponent(sphereTransform)
sphereEntity.addComponent(material)
return rootEntity
def __init__(self, **kwargs):
"""Initialize trackball"""
super(Trackball, self).__init__()
self._rotation = kwargs.get("rotation", QQuaternion())
self._angularVelocity = kwargs.get("velocity", 0.0)
self._axis = kwargs.get("axis", QVector3D(0.0, 1.0, 0.0))
self._mode = kwargs.get("mode", self.TrackballMode.Spherical)
self._lastPos = QPointF()
self._lastTime = QTime.currentTime()
self._paused = kwargs.get("paused", False)
self._pressed = False
def smooth_angle_bone(bone_frame_dic, smooth_times, target_bones):
# 関節の角度円滑化
for bone_name in target_bones:
for n in range(smooth_times):
for frame in range(len(bone_frame_dic[bone_name])):
if frame >= 2:
prev2_bf = bone_frame_dic[bone_name][frame - 2]
prev1_bf = bone_frame_dic[bone_name][frame - 1]
now_bf = bone_frame_dic[bone_name][frame]
if prev2_bf != now_bf.rotation:
# 角度が違っていたら、球形補正開始
prev1_bf.rotation = QQuaternion.slerp(
prev2_bf.rotation, now_bf.rotation, 0.5)
def rotate(self, angles, center):
"""
Rotate by euler angles at center
-----------------------
In: angle -> QVector3D
center -> QVector3D
-----------------------
"""
eulers = QQuaternion.fromEulerAngles(angles)
R, T = QMatrix4x4(), QMatrix4x4()
R.rotate(eulers)
T.translate(center)
self._modelView = T * R * T.inverted(
)[0] * self.modelView * self.CAMERA_ROT_STEP
def position_to_frame_lower_calc(frame, pos, slope_motion):
direction = pos[0] - pos[7]
up = QVector3D.crossProduct(direction, (pos[4] - pos[1]))
lower_body_orientation = QQuaternion.fromDirection(direction, up)
initial = QQuaternion.fromDirection(QVector3D(0, -1, 0),
QVector3D(0, 0, 1))
lower_body_rotation = lower_body_orientation * initial.inverted()
# 傾き補正
lower_correctqq = QQuaternion()
# 傾きデータがある場合、補正をかける
if slope_motion is not None:
# Y軸の回転具合を求める
y_degree = (180 - abs(lower_body_rotation.toEulerAngles().y())) / 180
# 一旦オイラー角に変換して、角度のかかり具合を補正し、再度クォータニオンに変換する
lower_correctqq = QQuaternion.fromEulerAngles(
slope_motion.frames["下半身"][0].rotation.toEulerAngles() *
y_degree).inverted()
lower_body_rotation = lower_correctqq * lower_body_rotation
return lower_body_rotation, lower_correctqq
class VmdBoneFrame():
def __init__(self, frame=0):
self.name = ''
self.format_name = ''
self.frame = frame
self.position = QVector3D(0, 0, 0)
self.rotation = QQuaternion()
self.complement = [
20, 20, 0, 0, 20, 20, 20, 20, 107, 107, 107, 107, 107, 107, 107,
107, 20, 20, 20, 20, 20, 20, 20, 107, 107, 107, 107, 107, 107, 107,
107, 0, 20, 20, 20, 20, 20, 20, 107, 107, 107, 107, 107, 107, 107,
107, 0, 0, 20, 20, 20, 20, 20, 107, 107, 107, 107, 107, 107, 107,
107, 0, 0, 0
]
self.key = True
def write(self, fout):
fout.write(self.name)
fout.write(bytearray([0 for i in range(len(self.name), 15)
])) # ボーン名15Byteの残りを\0で埋める
fout.write(struct.pack('<L', self.frame))
fout.write(struct.pack('<f', self.position.x()))
fout.write(struct.pack('<f', self.position.y()))
fout.write(struct.pack('<f', self.position.z()))
v = self.rotation.toVector4D()
fout.write(struct.pack('<f', v.x()))
fout.write(struct.pack('<f', v.y()))
fout.write(struct.pack('<f', v.z()))
fout.write(struct.pack('<f', v.w()))
# print(self.complement)
# print(b''.join(self.complement))
# print([ c.encode() for c in self.complement ])
# s = struct.Struct("64s")
# packed_value = struct.pack("64s", [ c.encode() for c in self.complement ])
# fout.write(packed_value)
# fout.write(struct.pack('64s', self.complement))
# fout.write([ c.encode('unicode_escape') for c in self.complement ])
# c = b''.join([ c.encode('unicode_escape') for c in self.complement ])
# print(c)
# print([ c.encode('unicode_escape') for c in self.complement ])
# fout.write(struct.pack('=64s', c))
# fout.write(struct.pack('=64s', [ c.encode('unicode_escape') for c in self.complement ][0]))
fout.write(bytearray(self.complement))
def copyFrom(self, camera):
"""Copy parameters from other camera"""
self._name = camera.name
self._lens = camera.lens
self._position = camera.position
self._fovy = camera.heightAngle
self._height = camera.height
self._near_distance = camera.nearDistance
self._far_distance = camera.farDistance
self._projection_matrix = QMatrix4x4(camera.projectionMatrix.data())
self._view_matrix = QMatrix4x4(camera.viewMatrix.data())
self._orientation = QMatrix4x4(camera.orientation.data())
self._rotation = QQuaternion(camera.rotation.toVector4D())
self._focal_distance = camera.focalDistance
self._aspect_ratio = camera.aspectRatio
def calc_limited_rotation(rot, minx, maxx, miny, maxy, minz, maxz):
euler = rot.toEulerAngles()
if euler.x() < minx:
euler.setX(minx)
elif euler.x() > maxx:
euler.setX(maxx)
if euler.y() < miny:
euler.setY(miny)
elif euler.y() > maxy:
euler.setY(maxy)
if euler.z() < minz:
euler.setZ(minz)
elif euler.z() > maxz:
euler.setZ(maxz)
return QQuaternion.fromEulerAngles(euler)
def position_to_frame_arm_one_side(frame, pos, pos_gan, upper_correctqq,
upper_body_rotation1, upper_body_rotation2,
gan_shoulder_initial_orientation,
shoulder_initial_orientation,
arm_initial_orientation, points, is_gan,
slope_motion, direction_name):
if pos_gan is not None and is_gan == True:
# 手(3dpose-gan採用)
return position_to_frame_shoulder_one_side_calc(
frame, pos_gan, QQuaternion(), upper_body_rotation1,
upper_body_rotation2, gan_shoulder_initial_orientation,
arm_initial_orientation, points, None, direction_name)
# 3d-pose-baseline の手FK
return position_to_frame_shoulder_one_side_calc(
frame, pos, upper_correctqq, upper_body_rotation1,
upper_body_rotation2, shoulder_initial_orientation,
arm_initial_orientation, points, slope_motion, direction_name)
