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

在理解四元数的SLERP和SQUAD的数学推导的基础上，使用C语言实现这两种插值方法；

参考：Understanding Quaternions | 3D Game Engine Programming (3dgep.com)实现文献中描述的SLERP和SQUAD四元数插值算法，用于机器人轨迹插值等。

文件声明

/**@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;	///<s
	float x;	///<xi
	float y;	///<yj
	float 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 q2
    is 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” around
	the 4D sphere which is not necessarily what we want. To solve this
	problem, we can test the result of the dot product and if it is
	negative, then we can negate one of the orientations.
	Negating the scalar and the vector part of the quaternion does not
	change the orientation that it represents but by doing this we guarantee
	that 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_i
	code = quaternion_squad_si(q1, q2, q3, &s1);
	CHECK_QUATERNION_ERROR_CODE(code);

	//calculate s_i+1
	code = quaternion_squad_si(q2, q3, q4, &s2);
	CHECK_QUATERNION_ERROR_CODE(code);

	//calculate slerp
	QUATERNION_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_i
	QUATERNION_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)