/**
* @ingroup quaternion
* @brief Linear interpolates between two quaternions. It will cause the resulting quaternion to move in a straight line "through the 3-sphere"
*
* @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_lerp(const quaternion *start, const quaternion *end, float percent, quaternion *qR)
{
float f1, f2;
/* 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;
}
f1 = 1.0f - percent;
f2 = percent;
/* 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;
}
/**
* @ingroup experimental
* @brief This code is suspect. Treats two quaternions sort of like 2 vector4's and then computes the cross-product between them.
*
*/
HYPAPI void quaternion_axis_between_EXP(const quaternion *self, const quaternion *qT, quaternion *qR)
{
quaternion axis;
axis = quaternion_cross_product_EXP(self, qT);
quaternion_set(qR, &axis);
quaternion_normalize(qR);
}
/**
* @ingroup experimental
* @brief this is an opinionated method (opinionated about what axis is yaw, pitch, roll and what is left/right/up/down
*
* @param self the quaternion
* @param ax the x axis
* @param ay the y axis
* @param az the z axis
*
*/
HYPAPI quaternion * quaternion_set_from_euler_anglesf3_EXP(quaternion *self, float ax, float ay, float az)
{
vector3 vx;
vector3 vy;
vector3 vz;
quaternion qx;
quaternion qy;
quaternion qz;
vector3_setf3(&vx, 1, 0, 0);
vector3_setf3(&vy, 0, 1, 0);
vector3_setf3(&vz, 0, 0, 1);
quaternion_set_from_axis_anglev3(&qx, &vx, ax);
quaternion_set_from_axis_anglev3(&qy, &vy, ay);
quaternion_set_from_axis_anglev3(&qz, &vz, az);
/* self = qx * qy * qz */
quaternion_multiply(&qx, &qy);
quaternion_multiply(&qx, &qz);
quaternion_set(self, &qx);
quaternion_normalize(self);
return self;
}
/**
* @ingroup vector3
* @brief Rotate a point by the quaternion. Returns the rotated point.
*
* \f$self= qT * self * conjugate(qT)\f$
*
* @param self the starting point
* @param qT the quaternion
*
*/
HYPAPI vector3 * vector3_rotate_by_quaternion(vector3 *self, const quaternion *qT)
{
quaternion qinverse;
quaternion q;
/* make the conjugate */
quaternion_set(&qinverse, qT);
quaternion_conjugate(&qinverse);
quaternion_set(&q, qT);
quaternion_multiplyv3(&q, self);
quaternion_multiply(&q, &qinverse);
self->x = q.x;
self->y = q.y;
self->z = q.z;
return self;
}
void
quaternion_inverse(Quaternion *ret, const Quaternion *quat)
{
/* double n = norm(quat); */
quaternion_set(ret, quat);
quaternion_conjugate(ret);
/* unit quaternion, so take conjugate */
quaternion_normalize(ret);
/* printf("Quaternion inverse: ret = {%f, %f, %f, %f}, quat = {%f, %f, %f, %f}\n",
ret->w, ret->x, ret->y, ret->z, quat->w, quat->x, quat->y, quat->z); */
}
/**
* @ingroup quaternion
* @brief in place multiplies the quaternion by the vector. The w element is handled a little differenly in this case. See source code.
*/
HYPAPI quaternion * quaternion_multiplyv3(quaternion *self, const vector3 *vT)
{
/* vT is the multiplicand */
quaternion r;
r.x = self->w*vT->x + self->y*vT->z - self->z*vT->y;
r.y = self->w*vT->y - self->x*vT->z + self->z*vT->x;
r.z = self->w*vT->z + self->x*vT->y - self->y*vT->x;
r.w = 0 - self->x*vT->x - self->y*vT->y - self->z*vT->z;
quaternion_set(self, &r); /* overwrite/save it */
return self;
}
/**
* @ingroup quaternion
* @brief in place multiplies the quaternion by a quaternion
*/
HYPAPI quaternion * quaternion_multiply(quaternion *self, const quaternion *qT)
{
/* qT is the multiplicand */
quaternion r;
r.x = self->w * qT->x + self->x * qT->w + self->y * qT->z - self->z * qT->y;
r.y = self->w * qT->y - self->x * qT->z + self->y * qT->w + self->z * qT->x;
r.z = self->w * qT->z + self->x * qT->y - self->y * qT->x + self->z * qT->w;
r.w = self->w * qT->w - self->x * qT->x - self->y * qT->y - self->z * qT->z;
quaternion_set(self, &r); /* overwrite/save it */
return self;
}
/** [quaternion inverse example] */
static char * test_quaternion_inverse()
{
quaternion qA;
quaternion qInverse;
quaternion qIdentity;
quaternion_set_from_axis_anglef3(&qA, 1.0f, 1.0f, 1.0f, HYP_TAU / 4.0f);
quaternion_set(&qInverse, &qA);
quaternion_inverse(&qInverse);
quaternion_multiply(&qA, &qInverse);
quaternion_normalize(&qA);
quaternion_identity(&qIdentity);
test_assert(quaternion_equals(&qA, &qIdentity));
return 0;
}
/**
* @ingroup vector3
* @brief Reflect a point by the quaternion. Returns the reflected point. (through origin)
*
* \f$self= qT * self * qT\f$
*
* @param qT the quaternion
* @param self the starting point that is rotated by qT
*
*/
HYPAPI vector3 * vector3_reflect_by_quaternion(vector3 *self, const quaternion *qT)
{
quaternion q;
quaternion_set(&q, qT);
quaternion_multiplyv3(&q, self);
quaternion_multiply(&q, qT);
/* this seems to be necessary */
quaternion_normalize(&q);
self->x = q.x;
self->y = q.y;
self->z = q.z;
return self;
}
static char * test_quaternion_multiply_identity()
{
quaternion qA, qB, q;
qA.x = 1;
qA.y = 2;
qA.z = 3;
qA.w = 4;
quaternion_identity(&qB);
quaternion_set(&q, &qA);
quaternion_multiply(&q, &qB);
test_assert(quaternion_equals(&q, &qA));
return 0;
}
/**
* @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;
}
