旋转向量转bvh旋转角度

目录

旋转向量转四元素

旋转向量转bvh旋转角度：

子节点从1  1 0 抓挠 0 1  1的例子：

---

答案不对

旋转向量转四元素

import numpy as np


def to_euler(x, order='zyx',face_front=False):
    
    q0 = x[...,0:1]
    q1 = x[...,1:2]
    q2 = x[...,2:3]
    q3 = x[...,3:4]
    
    if order == 'zyx':
    
        return np.concatenate([
            np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3)),
            np.arcsin((2 * (q0 * q2 - q3 * q1)).clip(-1,1)),
            np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2))], axis=-1)
            
    elif order == 'yzx':
    
        return np.concatenate([
            np.arctan2(2 * (q2 * q0 - q1 * q3),  q1 * q1 - q2 * q2 - q3 * q3 + q0 * q0),
            np.arcsin((2 * (q1 * q2 + q3 * q0)).clip(-1,1)), 
            np.arctan2(2 * (q1 * q0 - q2 * q3), -q1 * q1 + q2 * q2 - q3 * q3 + q0 * q0)],axis=-1)
            
    elif order == 'zxy':

        return np.concatenate([
            np.arctan2(2 * (q0 * q3 - q1 * q2), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3),
            np.arcsin((2 * (q0 * q1 + q2 * q3)).clip(-1,1)),
            np.arctan2(2 * (q0 * q2 - q1 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3)], axis=-1)

    elif order == 'yxz':
        
        data= np.concatenate([
            np.arctan2(2 * (q1 * q3 + q0 * q2), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3),
            np.arcsin((2 * (q0 * q1 - q2 * q3)).clip(-1,1)),
            np.arctan2(2 * (q1 * q2 + q0 * q3), q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3)], axis=-1)

        data2 = from_euler(data)
        return data
        
    else:
        raise NotImplementedError('Cannot convert from ordering %s' % order)

def from_angle_axis(angle, axis):
    c = np.cos(angle / 2.0)[..., None]
    s = np.sin(angle / 2.0)[..., None]
    q = np.concatenate([c, s * axis], axis=-1)
    return q

def from_axis_angle(rots):
    angle = np.linalg.norm(rots, axis=-1)
    axis = rots / angle[..., None]
    result = np.nan_to_num(axis)
    return from_angle_axis(angle, result)

# 绕 z 轴旋转 90 度的旋转向量
rot_vector = np.array([0, 0, np.pi / 2])

# 转换为四元数
quat = from_axis_angle(rot_vector)
print(quat)

旋转向量转bvh旋转角度：

子节点从1  1 0 旋转到 0 1 1

答案是90 90 0 ，但是算出来不对。

    rots = rots_o.reshape(rots_o.shape[0], -1, 3)
    rots = quat.from_axis_angle(rots)

    positions[:, 0] = trans#*scale_v

    order = "zyx"  # rotation顺序
    # order = "zxy"  # rotation顺序
    rotations = np.degrees(quat.to_euler(rots, order=order))

子节点从1  1 0 抓挠 0 1  1的例子：

import numpy as np

def from_angle_axis(angle, axis):
    c = np.cos(angle / 2.0)[..., None]
    s = np.sin(angle / 2.0)[..., None]
    q = np.concatenate([c, s * axis], axis=-1)
    return q

def from_axis_angle(rots):
    angle = np.linalg.norm(rots, axis=-1)
    axis = rots / angle[..., None]
    result = np.nan_to_num(axis)
    return from_angle_axis(angle, result)

# 定义初始位置和目标位置
initial_position = np.array([1, 1, 0])
target_position = np.array([0, 1, 1])

# 计算旋转轴和旋转角度
rotation_axis = np.cross(initial_position, target_position)
rotation_angle = np.arccos(np.dot(initial_position, target_position) / (np.linalg.norm(initial_position) * np.linalg.norm(target_position)))

# 使用 from_axis_angle 函数计算四元数
quat = from_axis_angle(rotation_axis * rotation_angle)
print(f'四元数: {quat}')