C++ 求子物体世界坐标

inline glm::quat Adjoint(glm::quat q)//求共轭四元数
{
    return glm::quat(q.w, -q.x, -q.y, -q.z);
}

inline glm::quat QuatMultiQuat(glm::quat q1, glm::quat q2)
{
    glm::vec3 q1_xyz =  glm::vec3(q1.x, q1.y, q1.z);
    glm::vec3 q2_xyz =  glm::vec3(q2.x, q2.y, q2.z);
    float w = q1.w * q2.w - glm::dot(q1_xyz, q2_xyz);
    glm::vec3 result = q1.w *  q2_xyz + q2.w * q1_xyz +  glm::cross(q1_xyz, q2_xyz);
    return glm::quat(w, result.x, result.y, result.z);
}

inline glm::vec3 QuatMultiVec3(glm::quat q1, glm::vec3 dir)
{
    glm::quat qDir = glm::quat(0.0, dir.x, dir.y, dir.z);
    glm::quat result = QuatMultiQuat(q1, qDir);
    result = QuatMultiQuat(result, Adjoint(q1));//得保证q是规范化的

    return glm::vec3(result.x, result.y, result.z);
}

glm::vec3 parent_world_position = glm::vec3(1.0, 2.0, 3.0);
glm::quat parent_world_quad = glm::quat(0.8247269000, 0.2234561000, 0.1628406000, 0.4933314000);
glm::vec3 child_local_position = glm::vec3(4.0, 5.0, 6.0);

//第一种方法直接使用glm四元数乘向量，更精准一点
glm::vec3 child_world_position_cal = parent_world_position + parent_world_quad * child_local_position;
LOGW("child_w_p_cal: %0.10f, %0.10f, %0.10f", child_world_position_cal.x, child_world_position_cal.y, child_world_position_cal.z);

//第二种方法使用自定义glm四元数乘向量
glm::vec3 child_world_position_cal2 = parent_world_position + QuatMultiVec3(parent_world_quad, child_local_position);
LOGW("child_world_position_cal2: %0.10f, %0.10f, %0.10f", child_world_position_cal2.x, child_world_position_cal2.y, child_world_position_cal2.z);
//Child world position: (2.0705400000, 6.3654570000, 10.5363600000)
//Child world calc position:(2.0705410000, 6.3654570000, 10.5363600000)
//child_w_p_cal: 2.0705406666, 6.3654556274, 10.5363616943
//child_world_position_cal2: 2.0705404282, 6.3654551506, 10.5363616943