Quaternion.h代码如下：
#ifndef QUATERNION_H_
#define QUATERNION_H_
#include "Vec3.h"
class Mat4;
/**
* Defines a 4-element quaternion that represents the orientation of an object in space.
*
* Quaternions are typically used as a replacement for euler angles and rotation matrices as a way to achieve smooth interpolation and avoid gimbal lock.
*
* Note that this quaternion class does not automatically keep the quaternion normalized. Therefore, care must be taken to normalize the quaternion when necessary, by calling the normalize method.
* This class provides three methods for doing quaternion interpolation: lerp, slerp, and squad.
*
* lerp (linear interpolation): the interpolation curve gives a straight line in quaternion space. It is simple and fast to compute. The only problem is that it does not provide constant angular velocity. Note that a constant velocity is not necessarily a requirement for a curve;
* slerp (spherical linear interpolation): the interpolation curve forms a great arc on the quaternion unit sphere. Slerp provides constant angular velocity;
* squad (spherical spline interpolation): interpolating between a series of rotations using slerp leads to the following problems:
* - the curve is not smooth at the control points;
* - the angular velocity is not constant;
* - the angular velocity is not continuous at the control points.
*
* Since squad is continuously differentiable, it remedies the first and third problems mentioned above.
* The slerp method provided here is intended for interpolation of principal rotations. It treats +q and -q as the same principal rotation and is at liberty to use the negative of either input. The resulting path is always the shorter arc.
*
* The lerp method provided here interpolates strictly in quaternion space. Note that the resulting path may pass through the origin if interpolating between a quaternion and its exact negative.
*
* As an example, consider the following quaternions:
*
* q1 = (0.6, 0.8, 0.0, 0.0),
* q2 = (0.0, 0.6, 0.8, 0.0),
* q3 = (0.6, 0.0, 0.8, 0.0), and
* q4 = (-0.8, 0.0, -0.6, 0.0).
* For the point p = (1.0, 1.0, 1.0), the following figures show the trajectories of p using lerp, slerp, and squad.
*/
/**
*定义表示空间中对象方向的四元数。
*
*四元数通常用作欧拉角和旋转矩阵的替换，以实现平滑插值和避免框架锁定。
*
*请注意，此四元数类不会自动保持四元数规范化。因此，必须注意在必要时通过调用normalize方法规范化四元数。
*这个类提供了三种四元数插值方法：lerp、slerp和squad。
*
*线性插值：插值曲线在四元数空间中给出一条直线。它计算简单快速。唯一的问题是它不能提供恒定的角速度。注意，恒定速度不一定是曲线的要求；
*球面线性插值：插值曲线在四元数单位球面上形成一条大圆弧。Slerp提供恒定角速度；
*squad（球面样条插值）：使用slerp在一系列旋转之间插值会导致以下问题：
*-控制点处曲线不光滑；
*-角速度不是恒定的；
*-控制点处的角速度不是连续的。
*
*由于squad是连续可微的，所以它解决了上述第一和第三个问题。
*这里提供的slerp方法用于主旋转的插值。它将+q和-q视为相同的主旋转，并且可以*使用任一输入的负数。生成的路径总是较短的弧。
*
*这里提供的lerp方法严格地在四元数空间中插值。请注意，如果在四元数与其精确负数之间进行插值，则生成的路径可能会通过原点。
*
*例如，考虑以下四元数：
*
*q1=（0.6，0.8，0.0，0.0），
*q2=（0.0，0.6，0.8，0.0），
*q3=（0.6、0.0、0.8、0.0）和
*q4=（-0.8，0.0，-0.6，0.0）。
*对于点p=（1.0，1.0，1.0），下图显示了p使用lerp、slerp和squad的轨迹。
*/
class Quaternion
{
friend class Curve;
friend class Transform;
public:
/**
* The x-value of the quaternion's vector component.
*/
float x;
/**
* The y-value of the quaternion's vector component.
*/
float y;
/**
* The z-value of the quaternion's vector component.
*/
float z;
/**
* The scalar component of the quaternion.
*/
float w;
/**
* Constructs a quaternion initialized to (0, 0, 0, 1).
*/
Quaternion();
/**
* Constructs a quaternion initialized to (0, 0, 0, 1).
*
* @param xx The x component of the quaternion.
* @param yy The y component of the quaternion.
* @param zz The z component of the quaternion.
* @param ww The w component of the quaternion.
*/
Quaternion(float xx, float yy, float zz, float ww);
/**
* Constructs a new quaternion from the values in the specified array.
*
* @param array The values for the new quaternion.
*/
Quaternion(float* array);
/**
* Constructs a quaternion equal to the rotational part of the specified matrix.
*
* @param m The matrix.
*/
Quaternion(const Mat4& m);
/**
* Constructs a quaternion equal to the rotation from the specified axis and angle.
*
* @param axis A vector describing the axis of rotation.
* @param angle The angle of rotation (in radians).
*/
Quaternion(const Vec3& axis, float angle);
/**
* Constructs a new quaternion that is a copy of the specified one.
*
* @param copy The quaternion to copy.
*/
Quaternion(const Quaternion& copy);
/**
* Destructor.
*/
~Quaternion();
/**
* Returns the identity quaternion.
*
* @return The identity quaternion.
*/
static const Quaternion& identity();
/**
* Returns the quaternion with all zeros.
*
* @return The quaternion.
*/
static const Quaternion& zero();
/**
* Determines if this quaternion is equal to the identity quaternion.
*
* @return true if it is the identity quaternion, false otherwise.
*/
bool isIdentity() const;
/**
* Determines if this quaternion is all zeros.
*
* @return true if this quaternion is all zeros, false otherwise.
*/
bool isZero() const;
/**
* Creates a quaternion equal to the rotational part of the specified matrix
* and stores the result in dst.
*
* @param m The matrix.
* @param dst A quaternion to store the conjugate in.
*/
static void createFromRotationMatrix(const Mat4& m, Quaternion* dst);
/**
* Creates this quaternion equal to the rotation from the specified axis and angle
* and stores the result in dst.
*
* @param axis A vector describing the axis of rotation.
* @param angle The angle of rotation (in radians).
* @param dst A quaternion to store the conjugate in.
*/
static void createFromAxisAngle(const Vec3& axis, float angle, Quaternion* dst);
/**
* Sets this quaternion to the conjugate(共轭) of itself.
*/
void conjugate();
/**
* Gets the conjugate of this quaternion.
*
*/
Quaternion getConjugated() const;
/**
* Sets this quaternion to the inverse of itself.
*
* Note that the inverse of a quaternion is equal to its conjugate
* when the quaternion is unit-length. For this reason, it is more
* efficient to use the conjugate method directly when you know your
* quaternion is already unit-length.
*
* @return true if the inverse can be computed, false otherwise.
*/
bool inverse();
/**
* Gets the inverse of this quaternion.
*
* Note that the inverse of a quaternion is equal to its conjugate
* when the quaternion is unit-length. For this reason, it is more
* efficient to use the conjugate method directly when you know your
* quaternion is already unit-length.
*/
Quaternion getInversed() const;
/**
* Multiplies this quaternion by the specified one and stores the result in this quaternion.
*
* @param q The quaternion to multiply.
*/
void multiply(const Quaternion& q);
/**
* Multiplies the specified quaternions and stores the result in dst.
*
* @param q1 The first quaternion.
* @param q2 The second quaternion.
* @param dst A quaternion to store the result in.
*/
static void multiply(const Quaternion& q1, const Quaternion& q2, Quaternion* dst);
/**
* Normalizes this quaternion to have unit length.
*
* If the quaternion already has unit length or if the length
* of the quaternion is zero, this method does nothing.
*/
void normalize();
/**
* Get the normalized quaternion.
*
* If the quaternion already has unit length or if the length
* of the quaternion is zero, this method simply copies
* this vector.
*/
Quaternion getNormalized() const;
/**
* Sets the elements of the quaternion to the specified values.
*
* @param xx The new x-value.
* @param yy The new y-value.
* @param zz The new z-value.
* @param ww The new w-value.
*/
void set(float xx, float yy, float zz, float ww);
/**
* Sets the elements of the quaternion from the values in the specified array.
*
* @param array An array containing the elements of the quaternion in the order x, y, z, w.
*/
void set(float* array);
/**
* Sets the quaternion equal to the rotational part of the specified matrix.
*
* @param m The matrix.
*/
void set(const Mat4& m);
/**
* Sets the quaternion equal to the rotation from the specified axis and angle.
*
* @param axis The axis of rotation.
* @param angle The angle of rotation (in radians).
*/
void set(const Vec3& axis, float angle);
/**
* Sets the elements of this quaternion to a copy of the specified quaternion.
*
* @param q The quaternion to copy.
*/
void set(const Quaternion& q);
/**
* Sets this quaternion to be equal to the identity quaternion.
*/
void setIdentity();
/**
* Converts this Quaternion4f to axis-angle notation. The axis is normalized.
*
* @param e The Vec3f which stores the axis.
*
* @return The angle (in radians).
*/
float toAxisAngle(Vec3* e) const;
/**
* Interpolates between two quaternions using linear interpolation.
*
* The interpolation curve for linear interpolation between
* quaternions gives a straight line in quaternion space.
*
* @param q1 The first quaternion.
* @param q2 The second quaternion.
* @param t The interpolation coefficient.
* @param dst A quaternion to store the result in.
*/
static void lerp(const Quaternion& q1, const Quaternion& q2, float t, Quaternion* dst);
/**
* Interpolates between two quaternions using spherical linear interpolation.
*
* Spherical linear interpolation provides smooth transitions between different
* orientations and is often useful for animating models or cameras in 3D.
*
* Note: For accurate interpolation, the input quaternions must be at (or close to) unit length.
* This method does not automatically normalize the input quaternions, so it is up to the
* caller to ensure they call normalize beforehand, if necessary.
*
* @param q1 The first quaternion.
* @param q2 The second quaternion.
* @param t The interpolation coefficient.
* @param dst A quaternion to store the result in.
*/
static void slerp(const Quaternion& q1, const Quaternion& q2, float t, Quaternion* dst);
/**
* Interpolates over a series of quaternions using spherical spline interpolation.
*
* Spherical spline interpolation provides smooth transitions between different
* orientations and is often useful for animating models or cameras in 3D.
*
* Note: For accurate interpolation, the input quaternions must be unit.
* This method does not automatically normalize the input quaternions,
* so it is up to the caller to ensure they call normalize beforehand, if necessary.
*
* @param q1 The first quaternion.
* @param q2 The second quaternion.
* @param s1 The first control point.
* @param s2 The second control point.
* @param t The interpolation coefficient.
* @param dst A quaternion to store the result in.
*/
static void squad(const Quaternion& q1, const Quaternion& q2, const Quaternion& s1, const Quaternion& s2, float t, Quaternion* dst);
/**
* Calculates the quaternion product of this quaternion with the given quaternion.
*
* Note: this does not modify this quaternion.
*
* @param q The quaternion to multiply.
* @return The quaternion product.
*/
inline const Quaternion operator*(const Quaternion& q) const;
/**
* Calculates the quaternion product of this quaternion with the given vec3.
* @param v The vec3 to multiply.
* @return The vec3 product.
*/
inline Vec3 operator*(const Vec3& v) const;
/**
* Multiplies this quaternion with the given quaternion.
*
* @param q The quaternion to multiply.
* @return This quaternion, after the multiplication occurs.
*/
inline Quaternion& operator*=(const Quaternion& q);
/** equals to Quaternion(0,0,0, 0) */
static const Quaternion ZERO;
private:
/**
* Interpolates between two quaternions using spherical linear interpolation.
*
* Spherical linear interpolation provides smooth transitions between different
* orientations and is often useful for animating models or cameras in 3D.
*
* Note: For accurate interpolation, the input quaternions must be at (or close to) unit length.
* This method does not automatically normalize the input quaternions, so it is up to the
* caller to ensure they call normalize beforehand, if necessary.
*
* @param q1x The x component of the first quaternion.
* @param q1y The y component of the first quaternion.
* @param q1z The z component of the first quaternion.
* @param q1w The w component of the first quaternion.
* @param q2x The x component of the second quaternion.
* @param q2y The y component of the second quaternion.
* @param q2z The z component of the second quaternion.
* @param q2w The w component of the second quaternion.
* @param t The interpolation coefficient.
* @param dstx A pointer to store the x component of the slerp in.
* @param dsty A pointer to store the y component of the slerp in.
* @param dstz A pointer to store the z component of the slerp in.
* @param dstw A pointer to store the w component of the slerp in.
*/
static void slerp(float q1x, float q1y, float q1z, float q1w, float q2x, float q2y, float q2z, float q2w, float t, float* dstx, float* dsty, float* dstz, float* dstw);
static void slerpForSquad(const Quaternion& q1, const Quaternion& q2, float t, Quaternion* dst);
};
#include "Quaternion.inl"
#endif
Quaternion.inl代码如下：
#include "Quaternion.h"
inline const Quaternion Quaternion::operator*(const Quaternion& q) const
{
Quaternion result(*this);
result.multiply(q);
return result;
}
inline Quaternion& Quaternion::operator*=(const Quaternion& q)
{
multiply(q);
return *this;
}
inline Vec3 Quaternion::operator*(const Vec3& v) const
{
Vec3 uv, uuv;
Vec3 qvec(x, y, z);
Vec3::cross(qvec, v, &uv);
Vec3::cross(qvec, uv, &uuv);
uv *= (2.0f * w);
uuv *= 2.0f;
return v + uv + uuv;
}
Quaternion.cpp代码如下：
#include "Quaternion.h"
#include<cstring>
#include <cmath>
const Quaternion Quaternion::ZERO(0.0f, 0.0f, 0.0f, 0.0f);
Quaternion::Quaternion()
: x(0.0f), y(0.0f), z(0.0f), w(1.0f)
{
}
Quaternion::Quaternion(float xx, float yy, float zz, float ww)
: x(xx), y(yy), z(zz), w(ww)
{
}
Quaternion::Quaternion(float* array)
{
set(array);
}
Quaternion::Quaternion(const Mat4& m)
{
set(m);
}
Quaternion::Quaternion(const Vec3& axis, float angle)
{
set(axis, angle);
}
Quaternion::Quaternion(const Quaternion& copy)
{
set(copy);
}
Quaternion::~Quaternion()
{
}
const Quaternion& Quaternion::identity()
{
static Quaternion value(0.0f, 0.0f, 0.0f, 1.0f);
return value;
}
const Quaternion& Quaternion::zero()
{
static Quaternion value(0.0f, 0.0f, 0.0f, 0.0f);
return value;
}
bool Quaternion::isIdentity() const
{
return x == 0.0f && y == 0.0f && z == 0.0f && w == 1.0f;
}
bool Quaternion::isZero() const
{
return x == 0.0f && y == 0.0f && z == 0.0f && w == 0.0f;
}
//void Quaternion::createFromRotationMatrix(const Mat4& m, Quaternion* dst)
//{
// m.getRotation(dst);
//}
void Quaternion::createFromAxisAngle(const Vec3& axis, float angle, Quaternion* dst)
{
GP_ASSERT(dst);
float halfAngle = angle * 0.5f;
float sinHalfAngle = sinf(halfAngle);
Vec3 normal(axis);
normal.normalize();
dst->x = normal.x * sinHalfAngle;
dst->y = normal.y * sinHalfAngle;
dst->z = normal.z * sinHalfAngle;
dst->w = cosf(halfAngle);
}
void Quaternion::conjugate()
{
x = -x;
y = -y;
z = -z;
//w = w;
}
Quaternion Quaternion::getConjugated() const
{
Quaternion q(*this);
q.conjugate();
return q;
}
bool Quaternion::inverse()
{
float n = x * x + y * y + z * z + w * w;
if (n == 1.0f)
{
x = -x;
y = -y;
z = -z;
//w = w;
return true;
}
// Too close to zero.
if (n < 0.000001f)
return false;
n = 1.0f / n;
x = -x * n;
y = -y * n;
z = -z * n;
w = w * n;
return true;
}
Quaternion Quaternion::getInversed() const
{
Quaternion q(*this);
q.inverse();
return q;
}
void Quaternion::multiply(const Quaternion& q)
{
multiply(*this, q, this);
}
void Quaternion::multiply(const Quaternion& q1, const Quaternion& q2, Quaternion* dst)
{
GP_ASSERT(dst);
float x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
float y = q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x;
float z = q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w;
float w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
dst->x = x;
dst->y = y;
dst->z = z;
dst->w = w;
}
void Quaternion::normalize()
{
float n = x * x + y * y + z * z + w * w;
// Already normalized.
if (n == 1.0f)
return;
n = std::sqrt(n);
// Too close to zero.
if (n < 0.000001f)
return;
n = 1.0f / n;
x *= n;
y *= n;
z *= n;
w *= n;
}
Quaternion Quaternion::getNormalized() const
{
Quaternion q(*this);
q.normalize();
return q;
}
void Quaternion::set(float xx, float yy, float zz, float ww)
{
this->x = xx;
this->y = yy;
this->z = zz;
this->w = ww;
}
void Quaternion::set(float* array)
{
GP_ASSERT(array);
x = array[0];
y = array[1];
z = array[2];
w = array[3];
}
void Quaternion::set(const Mat4& m)
{
Quaternion::createFromRotationMatrix(m, this);
}
void Quaternion::set(const Vec3& axis, float angle)
{
Quaternion::createFromAxisAngle(axis, angle, this);
}
void Quaternion::set(const Quaternion& q)
{
this->x = q.x;
this->y = q.y;
this->z = q.z;
this->w = q.w;
}
void Quaternion::setIdentity()
{
x = 0.0f;
y = 0.0f;
z = 0.0f;
w = 1.0f;
}
float Quaternion::toAxisAngle(Vec3* axis) const
{
GP_ASSERT(axis);
Quaternion q(x, y, z, w);
q.normalize();
axis->x = q.x;
axis->y = q.y;
axis->z = q.z;
axis->normalize();
return (2.0f * std::acos(q.w));
}
void Quaternion::lerp(const Quaternion& q1, const Quaternion& q2, float t, Quaternion* dst)
{
GP_ASSERT(dst);
GP_ASSERT(!(t < 0.0f || t > 1.0f));
if (t == 0.0f)
{
memcpy(dst, &q1, sizeof(float) * 4);
return;
}
else if (t == 1.0f)
{
memcpy(dst, &q2, sizeof(float) * 4);
return;
}
float t1 = 1.0f - t;
dst->x = t1 * q1.x + t * q2.x;
dst->y = t1 * q1.y + t * q2.y;
dst->z = t1 * q1.z + t * q2.z;
dst->w = t1 * q1.w + t * q2.w;
}
void Quaternion::slerp(const Quaternion& q1, const Quaternion& q2, float t, Quaternion* dst)
{
GP_ASSERT(dst);
slerp(q1.x, q1.y, q1.z, q1.w, q2.x, q2.y, q2.z, q2.w, t, &dst->x, &dst->y, &dst->z, &dst->w);
}
void Quaternion::squad(const Quaternion& q1, const Quaternion& q2, const Quaternion& s1, const Quaternion& s2, float t, Quaternion* dst)
{
GP_ASSERT(!(t < 0.0f || t > 1.0f));
Quaternion dstQ(0.0f, 0.0f, 0.0f, 1.0f);
Quaternion dstS(0.0f, 0.0f, 0.0f, 1.0f);
slerpForSquad(q1, q2, t, &dstQ);
slerpForSquad(s1, s2, t, &dstS);
slerpForSquad(dstQ, dstS, 2.0f * t * (1.0f - t), dst);
}
void Quaternion::slerp(float q1x, float q1y, float q1z, float q1w, float q2x, float q2y, float q2z, float q2w, float t, float* dstx, float* dsty, float* dstz, float* dstw)
{
// Fast slerp implementation by kwhatmough:
// It contains no division operations, no trig, no inverse trig
// and no sqrt. Not only does this code tolerate small constraint
// errors in the input quaternions, it actually corrects for them.
GP_ASSERT(dstx && dsty && dstz && dstw);
GP_ASSERT(!(t < 0.0f || t > 1.0f));
if (t == 0.0f)
{
*dstx = q1x;
*dsty = q1y;
*dstz = q1z;
*dstw = q1w;
return;
}
else if (t == 1.0f)
{
*dstx = q2x;
*dsty = q2y;
*dstz = q2z;
*dstw = q2w;
return;
}
if (q1x == q2x && q1y == q2y && q1z == q2z && q1w == q2w)
{
*dstx = q1x;
*dsty = q1y;
*dstz = q1z;
*dstw = q1w;
return;
}
float halfY, alpha, beta;
float u, f1, f2a, f2b;
float ratio1, ratio2;
float halfSecHalfTheta, versHalfTheta;
float sqNotU, sqU;
float cosTheta = q1w * q2w + q1x * q2x + q1y * q2y + q1z * q2z;
// As usual in all slerp implementations, we fold theta.
alpha = cosTheta >= 0 ? 1.0f : -1.0f;
halfY = 1.0f + alpha * cosTheta;
// Here we bisect the interval, so we need to fold t as well.
f2b = t - 0.5f;
u = f2b >= 0 ? f2b : -f2b;
f2a = u - f2b;
f2b += u;
u += u;
f1 = 1.0f - u;
// One iteration of Newton to get 1-cos(theta / 2) to good accuracy.
halfSecHalfTheta = 1.09f - (0.476537f - 0.0903321f * halfY) * halfY;
halfSecHalfTheta *= 1.5f - halfY * halfSecHalfTheta * halfSecHalfTheta;
versHalfTheta = 1.0f - halfY * halfSecHalfTheta;
// Evaluate series expansions of the coefficients.
sqNotU = f1 * f1;
ratio2 = 0.0000440917108f * versHalfTheta;
ratio1 = -0.00158730159f + (sqNotU - 16.0f) * ratio2;
ratio1 = 0.0333333333f + ratio1 * (sqNotU - 9.0f) * versHalfTheta;
ratio1 = -0.333333333f + ratio1 * (sqNotU - 4.0f) * versHalfTheta;
ratio1 = 1.0f + ratio1 * (sqNotU - 1.0f) * versHalfTheta;
sqU = u * u;
ratio2 = -0.00158730159f + (sqU - 16.0f) * ratio2;
ratio2 = 0.0333333333f + ratio2 * (sqU - 9.0f) * versHalfTheta;
ratio2 = -0.333333333f + ratio2 * (sqU - 4.0f) * versHalfTheta;
ratio2 = 1.0f + ratio2 * (sqU - 1.0f) * versHalfTheta;
// Perform the bisection and resolve the folding done earlier.
f1 *= ratio1 * halfSecHalfTheta;
f2a *= ratio2;
f2b *= ratio2;
alpha *= f1 + f2a;
beta = f1 + f2b;
// Apply final coefficients to a and b as usual.
float w = alpha * q1w + beta * q2w;
float x = alpha * q1x + beta * q2x;
float y = alpha * q1y + beta * q2y;
float z = alpha * q1z + beta * q2z;
// This final adjustment to the quaternion's length corrects for
// any small constraint error in the inputs q1 and q2 But as you
// can see, it comes at the cost of 9 additional multiplication
// operations. If this error-correcting feature is not required,
// the following code may be removed.
f1 = 1.5f - 0.5f * (w * w + x * x + y * y + z * z);
*dstw = w * f1;
*dstx = x * f1;
*dsty = y * f1;
*dstz = z * f1;
}
void Quaternion::slerpForSquad(const Quaternion& q1, const Quaternion& q2, float t, Quaternion* dst)
{
GP_ASSERT(dst);
// cos(omega) = q1 * q2;
// slerp(q1, q2, t) = (q1*sin((1-t)*omega) + q2*sin(t*omega))/sin(omega);
// q1 = +- q2, slerp(q1,q2,t) = q1.
// This is a straight-forward implementation of the formula of slerp. It does not do any sign switching.
float c = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
if (std::abs(c) >= 1.0f)
{
dst->x = q1.x;
dst->y = q1.y;
dst->z = q1.z;
dst->w = q1.w;
return;
}
float omega = std::acos(c);
float s = std::sqrt(1.0f - c * c);
if (std::abs(s) <= 0.00001f)
{
dst->x = q1.x;
dst->y = q1.y;
dst->z = q1.z;
dst->w = q1.w;
return;
}
float r1 = std::sin((1 - t) * omega) / s;
float r2 = std::sin(t * omega) / s;
dst->x = (q1.x * r1 + q2.x * r2);
dst->y = (q1.y * r1 + q2.y * r2);
dst->z = (q1.z * r1 + q2.z * r2);
dst->w = (q1.w * r1 + q2.w * r2);
}