机器人学：四元数插值方法SLERP和SQUAD的C语言实现

文件声明

/**@file  quaternion.c
* @brief    SLERP SQUAD
* @details
* @author      Emperor_Yang
* @date        2023-2-26
* @version     V1.0
* @copyright   Copyright (c) 2050
*/

使用到的基本运算

#include <math.h>
#include <float.h>
#include <stdint.h>
#include <memory.h>///<quaternion struct
typedef struct QUATERNION
{float s;   ///<sfloat x;    ///<xifloat y;   ///<yjfloat z;   ///<zk
}QUATERNION_t;///<error code
typedef enum QUATERNION_ERROR_CODE
{QUATERNION_SUCCESS = 0x00,QUATERNION_INPUT_ERROR = 0x01,QUATERNION_MEMORY_ERROR = 0x02
}QUATERNION_ERROR_CODE_t;#define CHECK_QUATERNION_ERROR_CODE(code) if(code != QUATERNION_SUCCESS) return code;
#define CHECK_QUATERNION_INPUT_POINTER(ptr) if(ptr == NULL) return QUATERNION_INPUT_ERROR;/**@brief quaternion norm
* @param[in]  *q1:quaternion pointer
* @return out:result
*/
float quaternion_norm(const QUATERNION_t* q1)
{float out = sqrtf(q1->s * q1->s + q1->x * q1->x + q1->y * q1->y + q1->z * q1->z);return out;
}/**@brief quaternion dot product
* @param[in]  *q1:quaternion pointer
* @param[in]  *q2:quaternion pointer
* @return out:result
*/
float quaternion_dot_product(const QUATERNION_t* q1, const QUATERNION_t* q2)
{float out = q1->s * q2->s + q1->x * q2->x + q1->y * q2->y + q1->z * q2->z;return out;
}/**@brief quaternion mult constant
* @param[in]  *q:quaternion pointer
* @param[in]  multiplier:multiplier
* @param[out]  *out:quaternion pointer
* @return out:result
*/
void quaternion_mult_constant(const QUATERNION_t* q, float multiplier, QUATERNION_t* out)
{out->s = q->s * multiplier;out->x = q->x * multiplier;out->y = q->y * multiplier;out->z = q->z * multiplier;return;
}/**@brief quaternion add
* @param[in]  *q1:quaternion pointer
* @param[in]  *q2:quaternion pointer
* @param[out]  out:quaternion
* @return out:result
*/
void quaternion_add(const QUATERNION_t* q1, const QUATERNION_t* q2, QUATERNION_t* out)
{out->s = q1->s + q2->s;out->x = q1->x + q2->x;out->y = q1->y + q2->y;out->z = q1->z + q2->z;return;
}/**@brief quaternion mult
* @param[in]  *q1:quaternion pointer
* @param[in]  *q2:quaternion pointer
* @param[out]  out:quaternion
* @return out:result
*/
QUATERNION_ERROR_CODE_t
quaternion_product(const QUATERNION_t* q1, const QUATERNION_t* q2, QUATERNION_t* out)
{CHECK_QUATERNION_INPUT_POINTER(q1);CHECK_QUATERNION_INPUT_POINTER(q1);out->s = q1->s * q2->s - q1->x * q2->x - q1->y * q2->y - q1->z * q2->z;out->x = q1->s * q2->x + q2->s * q1->x + q1->y * q2->z - q2->y * q1->z;out->y = q1->s * q2->y + q2->s * q1->y + q1->z * q2->x - q2->z * q1->x;out->z = q1->s * q2->z + q2->s * q1->z + q1->x * q2->y - q2->x * q1->y;return QUATERNION_SUCCESS;
}/**@brief quaternion conjugate
* @param[in]  * q:quaternion pointer
* @param[out]  *out:quaternion pointer
* @return
*/
void quaternion_conjugate(const QUATERNION_t* q, QUATERNION_t* out)
{out->s = q->s;out->x = - q->x;out->y = - q->y;out->z = - q->z;return;
}/**@brief quaternion inverse
* @param[in]  *q:quaternion pointer
* @param[out]  *out:quaternion pointer
* @return
*/
void quaternion_inverse(const QUATERNION_t* q, QUATERNION_t* out)
{float q_norm = quaternion_norm(q);float multi = 1.0f / (q_norm * q_norm);QUATERNION_t q_conjugate = { 0 };quaternion_conjugate(q, &q_conjugate);quaternion_mult_constant(&q_conjugate, multi, out);return;
}/**@brief quaternion is norm
* @param[in]  *q:quaternion pointer
* @param[out]  *out:quaternion pointer
* @return
*/
bool quaternion_is_norm(const QUATERNION_t* q)
{float q_norm = quaternion_norm(q);if (fabsf(q_norm - 1.0f) <= FLT_EPSILON){return true;}return false;
}/**@brief quaternion normalize
* @param[in]  *q:quaternion pointer
* @param[out]  *out:quaternion pointer
* @return
*/
QUATERNION_ERROR_CODE_t
quaternion_normalize(const QUATERNION_t* q, QUATERNION_t*out)
{CHECK_QUATERNION_INPUT_POINTER(q);CHECK_QUATERNION_INPUT_POINTER(out);float q_norm = quaternion_norm(q);if (q_norm < FLT_EPSILON)//Avoid zero division{return QUATERNION_INPUT_ERROR;}float norm_inverse = 1.0f / q_norm;out->s = q->s * norm_inverse;out->x = q->x * norm_inverse;out->y = q->y * norm_inverse;out->z = q->z * norm_inverse;return QUATERNION_SUCCESS;
}/**@brief quaternion log
* @param[in]  *q:quaternion pointer
* @param[out]  *out:quaternion pointer
* @return QUATERNION_ERROR_CODE_t
*/
QUATERNION_ERROR_CODE_t
quaternion_log(const QUATERNION_t* q, QUATERNION_t* out)
{CHECK_QUATERNION_INPUT_POINTER(q);CHECK_QUATERNION_INPUT_POINTER(out);if (!quaternion_is_norm(q)){return QUATERNION_INPUT_ERROR;}float theta = acosf(q->s);if (theta < FLT_EPSILON)//Avoid zero division{return QUATERNION_INPUT_ERROR;}out->s = 0.0f;float sin_theta_inverse = 1.0f / sinf(theta);out->x = q->x * theta * sin_theta_inverse;out->y = q->y * theta * sin_theta_inverse;out->z = q->z * theta * sin_theta_inverse;return QUATERNION_SUCCESS;
}/**@brief quaternion_exp
* @param[in]  *q:quaternion pointer
* @param[out]  *out:quaternion pointer
* @return QUATERNION_ERROR_CODE_t
*/
QUATERNION_ERROR_CODE_t
quaternion_exp(const QUATERNION_t* q, QUATERNION_t* out)
{CHECK_QUATERNION_INPUT_POINTER(q);CHECK_QUATERNION_INPUT_POINTER(out);if (q->s > FLT_EPSILON){return QUATERNION_INPUT_ERROR;}float norm = quaternion_norm(q);out->s = cosf(norm);if (norm <= FLT_EPSILON){out->x = q->x;out->y = q->y;out->z = q->z;}else{QUATERNION_t norm_q = { 0 };QUATERNION_ERROR_CODE_t code = quaternion_normalize(q, &norm_q);CHECK_QUATERNION_ERROR_CODE(code);float sin_norm = sinf(norm);out->x = norm_q.x * sin_norm;out->y = norm_q.y * sin_norm;out->z = norm_q.z * sin_norm;}return QUATERNION_SUCCESS;
}

SLERP

/**@brief quaternion_slerp
* @param[in]  *q1:quaternion pointer
* @param[in]  *q2:quaternion pointer
* @param[in]  t:time
* @param[out]  *qt:quanterion pointer
* @return
*/
QUATERNION_ERROR_CODE_t
quaternion_slerp(const QUATERNION_t* q1, const QUATERNION_t* q2, float t, QUATERNION_t* qt)
{float dot = quaternion_dot_product(q1, q2);float scale_left = 0.0f;float scale_right = 0.0f;/*The other problem arises when the angular difference between q1 and q2is very small then sinθ becomes 0. If this happens, then we will get an undefined result when we divide by sinθ. In this case, we can fall-back to using linear interpolation between q1and q2.*/if (fabsf(dot) > (1 - FLT_EPSILON)){scale_left = 1 - t;scale_right = t;}else{float cos_theta = dot / (quaternion_norm(q1) * quaternion_norm(q2));float theta = acosf(fabsf(cos_theta));scale_left = (sinf(1 - t) * theta) / (sinf(theta));scale_right = sinf(t * theta) / sinf(theta);}/*First, if the quaternion dot-product results in a negative value,then the resulting interpolation will travel the “long-way” aroundthe 4D sphere which is not necessarily what we want. To solve thisproblem, we can test the result of the dot product and if it isnegative, then we can negate one of the orientations.Negating the scalar and the vector part of the quaternion does notchange the orientation that it represents but by doing this we guaranteethat the rotation will be applied in the “shortest” path.*/if (dot < FLT_EPSILON){scale_left = -scale_left;}QUATERNION_t left = { 0 };QUATERNION_t right = { 0 };quaternion_mult_constant(q1, scale_left, &left);quaternion_mult_constant(q2, scale_right, &right);quaternion_add(&left, &right, qt);return QUATERNION_SUCCESS;
}

SQUAD

static QUATERNION_ERROR_CODE_t
quaternion_squad_si(const QUATERNION_t* q1, const QUATERNION_t* q2,const QUATERNION_t* q3, QUATERNION_t* out);/**@brief quaternion_squad
* @param[in]  *q1:quaternion pointer
* @param[in]  *q2:quaternion pointer
* @param[in]  *q3:quaternion pointer
* @param[in]  *q4:quaternion pointer
* @param[in]  t:time
* @param[out]  *qt:quanterion pointer
* @return
*/
QUATERNION_ERROR_CODE_t
quaternion_squad(const QUATERNION_t* q1, const QUATERNION_t* q2,const QUATERNION_t* q3, const QUATERNION_t* q4,float t, QUATERNION_t* qt)
{CHECK_QUATERNION_INPUT_POINTER(q1);CHECK_QUATERNION_INPUT_POINTER(q2);CHECK_QUATERNION_INPUT_POINTER(q3);CHECK_QUATERNION_INPUT_POINTER(q4);QUATERNION_ERROR_CODE_t code = QUATERNION_SUCCESS;QUATERNION_t s1 = { 0 };QUATERNION_t s2 = { 0 };//calculate s_icode = quaternion_squad_si(q1, q2, q3, &s1);CHECK_QUATERNION_ERROR_CODE(code);//calculate s_i+1code = quaternion_squad_si(q2, q3, q4, &s2);CHECK_QUATERNION_ERROR_CODE(code);//calculate slerpQUATERNION_t slerp1 = { 0 };QUATERNION_t slerp2 = { 0 };code = quaternion_slerp(q2, q3, t, &slerp1);CHECK_QUATERNION_ERROR_CODE(code);code = quaternion_slerp(&s1, &s2, t, &slerp2);CHECK_QUATERNION_ERROR_CODE(code);code = quaternion_slerp(&slerp1, &slerp2, 2 * t * (1.0f - t), qt);CHECK_QUATERNION_ERROR_CODE(code);return QUATERNION_SUCCESS;
}/**@brief quaternion squad si
* @param[in]  *q1:quaternion pointer
* @param[in]  *q2:quaternion pointer
* @param[in]  *q3:quaternion pointer
* @param[out]  *out:quanterion pointer
* @return
*/
static QUATERNION_ERROR_CODE_t
quaternion_squad_si(const QUATERNION_t* q1, const QUATERNION_t* q2,const QUATERNION_t* q3, QUATERNION_t* out)
{QUATERNION_ERROR_CODE_t code = QUATERNION_SUCCESS;//calculate s_iQUATERNION_t q2_inverse = { 0 };QUATERNION_t q3q2_mult = { 0 };QUATERNION_t q1q2_mult = { 0 };QUATERNION_t q3q2_mult_log = { 0 };QUATERNION_t q1q2_mult_log = { 0 };QUATERNION_t q3q2_q1q2_add = { 0 };QUATERNION_t q3q2_q1q2_add_exp = { 0 };quaternion_inverse(q2, &q2_inverse);code = quaternion_product(q3, &q2_inverse, &q3q2_mult);CHECK_QUATERNION_ERROR_CODE(code);code = quaternion_log(&q3q2_mult, &q3q2_mult_log);CHECK_QUATERNION_ERROR_CODE(code);code = quaternion_product(q1, &q2_inverse, &q1q2_mult);CHECK_QUATERNION_ERROR_CODE(code);code = quaternion_log(&q1q2_mult, &q1q2_mult_log);CHECK_QUATERNION_ERROR_CODE(code);quaternion_add(&q3q2_mult_log, &q1q2_mult_log, &q3q2_q1q2_add);quaternion_mult_constant(&q3q2_q1q2_add, -0.25f, &q3q2_q1q2_add);code = quaternion_exp(&q3q2_q1q2_add, &q3q2_q1q2_add_exp);CHECK_QUATERNION_ERROR_CODE(code);code = quaternion_product(&q3q2_q1q2_add_exp, q2, out);CHECK_QUATERNION_ERROR_CODE(code);return QUATERNION_SUCCESS;
}

参考：

Understanding Quaternions | 3D Game Engine Programming (3dgep.com)
3D Math Primer for Game Programmers (Matrices) | 3D Game Engine Programming (3dgep.com)
Doxygen 注释语法规范 - schips - 博客园 (cnblogs.com)

机器人学：四元数插值方法SLERP和SQUAD的C语言实现相关推荐

四元数插值方法Slerp/Squad/Spicv/Sping知识总结思维导图
四元数插值方法Slerp/Squad/Spicv/Sping知识总结思维导图最近在学习思维导图,闲来无事,就把之前写过的博客,四元数插值方法Slerp.Squad.Spicv和Sping知识点总结整 ...
三维空间刚体运动4-5：四元数多点离散数值解插值方法：Sping
三维空间刚体运动4-5:四元数多点离散数值解插值方法:Sping 1. 正切曲率κ(γ,t)\kappa(\gamma, t)κ(γ,t)在H1H_{1}H1上的离散数值解--Sping 1.1 离 ...
三维空间刚体运动4-4：四元数多点连续解析解插值方法：Spicv
三维空间刚体运动4-4:四元数多点连续解析解插值方法:Spicv 1. 总述:多点旋转插值的数学方法 2. 插值曲线及其连续性 2.1 插值曲线定义 2.2 插值曲线连续性的讨论 3. 最优插值曲线 ...
单位四元数多姿态插值（squad）
squad是Shoemake在1987年提出的一种比贝塞尔曲线更高效的近似算法,之前想运用它进行机械臂末端的多姿态平滑插值,但是上网搜关于squad姿态插补的算法,没找着,所以自己写了个C语言版本 ...
3D 中的方位与角位移(旋转矩阵、欧拉角、四元数)
文章目录一.3D 中的方位与角位移 1. 欧拉角 (Euler angles) 2. 四元数的相关知识 2.1 复数 2.2 欧拉旋转定理 2.3 三维空间旋转的拆分 3. 四元数 (Quatern ...
三维空间刚体运动4-1：四元数表示旋转（各形式相互转换加代码）
三维空间刚体运动4-1:四元数表示变换(各形式相互转换加代码) 1. 四元数的定义 1.1 为什么使用四元数 1.2 复数与四元数 1.3 四元数的形式 2. 四元数的运算 2.1 基础运算 2.2 ...
Understanding Quaternions 中文翻译《理解四元数》
Tags: math, quaternion 原文地址:http://www.3dgep.com/understanding-quaternions/ 正文在这篇文章中我会尝试用简单的方式去解释四元 ...
刚体运动学-四元数插值
前言之前对写了一篇关于刚体运动学相关知识博客:刚体运动学--欧拉角.四元数.旋转矩阵,本篇博客就举例来说明,如何在运动捕捉数据中进行四元数插值. 国际惯例,参考博客: 探讨:向量(方向)之间的插值- ...
OpenGL相机自由移动旋转缩放，四元数,欧拉角，LookAt
OpenGL相机自由移动旋转缩放,四元数,欧拉角,LookAt 定义相机摄像机位置右轴上轴 Look At 自由移动相机左右移动移动速度视角移动欧拉角通过欧拉角计算实际的方向向量缩放 ...