TEST_F(QuaternionTest, sperical_linear_interpolation_between_from_ant_to_with_t_equal_one_return_to)
{
auto from = create_random_quaternion();
auto to = create_random_quaternion();
quaternion_normalize(from);
quaternion_normalize(to);
const auto res = quaternion_slerp(from, to, 1.0);
EXPECT_EQ(to.w(), res.w());
EXPECT_EQ(to.x(), res.x());
EXPECT_EQ(to.y(), res.y());
EXPECT_EQ(to.z(), res.z());
}
TEST_F(QuaternionTest, sperical_linear_interpolation_makes_correct_quaternion)
{
auto from = create_random_quaternion();
auto to = create_random_quaternion();
quaternion_normalize(from);
quaternion_normalize(to);
const auto t =(rand() % 400) / 400.0;
const auto res = quaternion_slerp(from, to, t);
const auto correct = slerp(from, to, t);
EXPECT_EQ(correct.w(), res.w());
EXPECT_EQ(correct.x(), res.x());
EXPECT_EQ(correct.y(), res.y());
EXPECT_EQ(correct.z(), res.z());
}
/**
* @ingroup quaternion
* @brief This computes the SLERP between two quaternions. It computes that absolute final position interopolated between start and end.
* This function computes shortest arc.
* If the start and end result in a negative dot product (reversed direction) then the SLERP will reverse direction.
*
* @param percent This refers to how far along we want to go toward end. 1.0 means go to the end. 0.0 means start. 1.1 is nonsense. Negative is nonsense.
* @param start The quaternion representing the starting orientation
* @param end The quaternion representing the final or ending orientation
* @param qR The resulting new orientation.
*
*/
HYPAPI quaternion * quaternion_slerp(const quaternion *start, const quaternion *end, float percent, quaternion *qR)
{
float dot;
float f1, f2;
float theta;
float s;
quaternion qneg;
/* if percent is 0, return start */
if(percent == 0.0f)
{
quaternion_set(qR, start);
return qR;
}
/* if percent is 1 return end */
if(percent == 1.0f)
{
quaternion_set(qR, end);
return qR;
}
/* how parallel are the quaternions (also the dot is the cosine) */
dot = quaternion_dot_product(start, end);
/* if they are close to parallel, use LERP
* - This avoids div/0
* - At small angles, the slerp and lerp are the same
*/
if((1.0f - HYP_ABS(dot)) < HYP_EPSILON)
{
quaternion_lerp(start, end, percent, qR);
return qR;
}
/* if dot is negative, they are "pointing" away from one another,
* use the shortest arc instead (reverse end and start)
* This has the effect of changing the direction of travel around the sphere
* beginning with "end" and going the other way around the sphere
*/
if(dot < 0.0f)
{
quaternion_set(&qneg, end);
/*quaternion_conjugate(&qneg);*/
quaternion_negate(&qneg);
quaternion_slerp(start, &qneg, percent, qR);
quaternion_negate(qR);
/*quaternion_conjugate(qR);*/
return qR;
}
/* keep the dot product in the range that acos can handle (shouldn't get here) */
HYP_CLAMP(dot, -1.0f, 1.0f);
theta = HYP_ACOS(dot); /* what is the angle between start and end in radians */
s = HYP_SIN(theta); /* cache */
f1 = HYP_SIN((1.0 - percent) * theta) / s; /* compute negative */
f2 = HYP_SIN(percent * theta) / s; /* compute positive */
/* this expanded form avoids calling quaternion_multiply and quaternion_add */
qR->w = f1 * start->w + f2 * end->w;
qR->x = f1 * start->x + f2 * end->x;
qR->y = f1 * start->y + f2 * end->y;
qR->z = f1 * start->z + f2 * end->z;
return qR;
}
static char * test_quaternion_slerp()
{
quaternion q, q1, q2, q3;
float angle;
angle = HYP_TAU / 4.0f;
quaternion_set_from_axis_anglev3(&q1, HYP_VECTOR3_UNIT_X, angle);
quaternion_set_from_axis_anglev3(&q2, HYP_VECTOR3_UNIT_X, angle * 1.1f);
quaternion_set_from_axis_anglev3(&q3, HYP_VECTOR3_UNIT_X, angle * 1.2f);
/* half-way */
quaternion_slerp(&q1, &q3, 0.5f, &q);
test_assert(quaternion_equals(&q, &q2));
/* none */
quaternion_slerp(&q1, &q3, 0.0f, &q);
test_assert(quaternion_equals(&q, &q1));
/* all the way */
quaternion_slerp(&q1, &q3, 1.0f, &q);
test_assert(quaternion_equals(&q, &q3));
/* swap order half-way */
quaternion_slerp(&q3, &q1, 0.5f, &q);
test_assert(quaternion_equals(&q, &q2));
/* swap order none */
quaternion_slerp(&q3, &q1, 0.0f, &q);
test_assert(quaternion_equals(&q, &q3));
/* swap order all the way */
quaternion_slerp(&q3, &q1, 1.0f, &q);
test_assert(quaternion_equals(&q, &q1));
/* go reverse around the sphere */
quaternion_set_from_axis_anglev3(&q1, HYP_VECTOR3_UNIT_X, angle);
quaternion_set_from_axis_anglev3(&q2, HYP_VECTOR3_UNIT_X, angle * 0.9f);
quaternion_set_from_axis_anglev3(&q3, HYP_VECTOR3_UNIT_X, angle * 0.8f);
/* go reverse half-way */
quaternion_slerp(&q1, &q3, 0.5f, &q);
test_assert(quaternion_equals(&q, &q2));
/* go reverse none */
quaternion_slerp(&q1, &q3, 0.0f, &q);
test_assert(quaternion_equals(&q, &q1));
/* go reverse all the way */
quaternion_slerp(&q1, &q3, 1.0f, &q);
test_assert(quaternion_equals(&q, &q3));
/* swap order reverse half-way */
quaternion_slerp(&q3, &q1, 0.5f, &q);
test_assert(quaternion_equals(&q, &q2));
/* swap order reverse none */
quaternion_slerp(&q3, &q1, 0.0f, &q);
test_assert(quaternion_equals(&q, &q3));
/* swap order reverse all the way */
quaternion_slerp(&q3, &q1, 1.0f, &q);
test_assert(quaternion_equals(&q, &q1));
return 0;
}
