You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
585 lines
16 KiB
585 lines
16 KiB
/**
|
|
Copyright 2013 BlackBerry Inc.
|
|
Copyright (c) 2013-2016 Chukong Technologies Inc.
|
|
Copyright (c) 2017-2023 Xiamen Yaji Software Co., Ltd.
|
|
|
|
Licensed under the Apache License, Version 2.0 (the "License");
|
|
you may not use this file except in compliance with the License.
|
|
You may obtain a copy of the License at
|
|
|
|
http://www.apache.org/licenses/LICENSE-2.0
|
|
|
|
Unless required by applicable law or agreed to in writing, software
|
|
distributed under the License is distributed on an "AS IS" BASIS,
|
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
See the License for the specific language governing permissions and
|
|
limitations under the License.
|
|
|
|
Original file from GamePlay3D: http://gameplay3d.org
|
|
|
|
This file was modified to fit the cocos2d-x project
|
|
*/
|
|
|
|
#include "math/Quaternion.h"
|
|
|
|
#include <cmath>
|
|
#include <cstring>
|
|
#include "base/Macros.h"
|
|
#include "math/Mat3.h"
|
|
#include "math/Math.h"
|
|
#include "math/Utils.h"
|
|
|
|
NS_CC_MATH_BEGIN
|
|
|
|
const Quaternion Quaternion::ZERO(0.F, 0.F, 0.F, 0.F);
|
|
|
|
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);
|
|
}
|
|
|
|
const Quaternion &Quaternion::identity() {
|
|
static Quaternion value(0.F, 0.F, 0.F, 1.F);
|
|
return value;
|
|
}
|
|
|
|
const Quaternion &Quaternion::zero() {
|
|
static Quaternion value(0.F, 0.F, 0.F, 0.F);
|
|
return value;
|
|
}
|
|
|
|
bool Quaternion::isIdentity() const {
|
|
return x == 0.F && y == 0.F && z == 0.F && w == 1.F;
|
|
}
|
|
|
|
bool Quaternion::isZero() const {
|
|
return x == 0.F && y == 0.F && z == 0.F && w == 0.F;
|
|
}
|
|
|
|
void Quaternion::createFromRotationMatrix(const Mat4 &m, Quaternion *dst) {
|
|
m.getRotation(dst);
|
|
}
|
|
|
|
void Quaternion::createFromAxisAngle(const Vec3 &axis, float angle, Quaternion *dst) {
|
|
CC_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::createFromAngleZ(float z, Quaternion *dst) {
|
|
CC_ASSERT(dst);
|
|
|
|
z *= mathutils::HALF_TO_RAD;
|
|
dst->x = dst->y = 0.F;
|
|
dst->z = sinf(z);
|
|
dst->w = cosf(z);
|
|
}
|
|
|
|
void Quaternion::conjugate() {
|
|
x = -x;
|
|
y = -y;
|
|
z = -z;
|
|
}
|
|
|
|
Quaternion Quaternion::getConjugated() const {
|
|
Quaternion q(*this);
|
|
q.conjugate();
|
|
return q;
|
|
}
|
|
|
|
void Quaternion::inverse() {
|
|
float dot = x * x + y * y + z * z + w * w;
|
|
float invDot = dot > 0.F ? 1.F / dot : 0.F;
|
|
|
|
x = -x * invDot;
|
|
y = -y * invDot;
|
|
z = -z * invDot;
|
|
w = w * invDot;
|
|
}
|
|
|
|
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) {
|
|
CC_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 len = x * x + y * y + z * z + w * w;
|
|
if (len > 0.F) {
|
|
len = 1.F / sqrt(len);
|
|
x *= len;
|
|
y *= len;
|
|
z *= len;
|
|
w *= len;
|
|
} else {
|
|
x = 0.F;
|
|
y = 0.F;
|
|
z = 0.F;
|
|
w = 0.F;
|
|
}
|
|
}
|
|
|
|
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(const float *array) {
|
|
CC_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.F;
|
|
y = 0.F;
|
|
z = 0.F;
|
|
w = 1.F;
|
|
}
|
|
|
|
float Quaternion::toAxisAngle(Vec3 *axis) const {
|
|
CC_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.F * std::acos(q.w));
|
|
}
|
|
|
|
void Quaternion::lerp(const Quaternion &q1, const Quaternion &q2, float t, Quaternion *dst) {
|
|
CC_ASSERT(dst);
|
|
CC_ASSERT(!(t < 0.F || t > 1.F));
|
|
|
|
float t1 = 1.F - 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 &a, const Quaternion &b, float t, Quaternion *dst) {
|
|
// benchmarks: http://jsperf.com/quaternion-slerp-implementations
|
|
|
|
float scale0 = 0;
|
|
float scale1 = 0;
|
|
float bx = b.x;
|
|
float by = b.y;
|
|
float bz = b.z;
|
|
float bw = b.w;
|
|
|
|
// calc cosine
|
|
float cosom = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
|
|
// adjust signs (if necessary)
|
|
if (cosom < 0.F) {
|
|
cosom = -cosom;
|
|
bx = -bx;
|
|
by = -by;
|
|
bz = -bz;
|
|
bw = -bw;
|
|
}
|
|
// calculate coefficients
|
|
if ((1.F - cosom) > 0.000001F) {
|
|
// standard case (slerp)
|
|
const float omega = acos(cosom);
|
|
const float sinom = sinf(omega);
|
|
scale0 = sinf((1.0F - t) * omega) / sinom;
|
|
scale1 = sinf(t * omega) / sinom;
|
|
} else {
|
|
// "from" and "to" quaternions are very close
|
|
// ... so we can do a linear interpolation
|
|
scale0 = 1.0F - t;
|
|
scale1 = t;
|
|
}
|
|
// calculate final values
|
|
dst->x = scale0 * a.x + scale1 * bx;
|
|
dst->y = scale0 * a.y + scale1 * by;
|
|
dst->z = scale0 * a.z + scale1 * bz;
|
|
dst->w = scale0 * a.w + scale1 * bw;
|
|
}
|
|
|
|
void Quaternion::sqlerp(const Quaternion &a, const Quaternion &b, const Quaternion &c, const Quaternion &d, float t, Quaternion *dst) {
|
|
Quaternion q1(0.F, 0.F, 0.F, 1.F);
|
|
Quaternion q2(0.F, 0.F, 0.F, 1.F);
|
|
|
|
slerp(a, d, t, &q1);
|
|
slerp(b, c, t, &q2);
|
|
slerp(q1, q2, 2.F * t * (1.F - t), dst);
|
|
}
|
|
|
|
void Quaternion::squad(const Quaternion &q1, const Quaternion &q2, const Quaternion &s1, const Quaternion &s2, float t, Quaternion *dst) {
|
|
CC_ASSERT(!(t < 0.F || t > 1.F));
|
|
|
|
Quaternion dstQ(0.F, 0.F, 0.F, 1.F);
|
|
Quaternion dstS(0.F, 0.F, 0.F, 1.F);
|
|
|
|
slerpForSquad(q1, q2, t, &dstQ);
|
|
slerpForSquad(s1, s2, t, &dstS);
|
|
slerpForSquad(dstQ, dstS, 2.F * t * (1.F - t), dst);
|
|
}
|
|
|
|
float Quaternion::angle(const Quaternion &a, const Quaternion &b) {
|
|
const auto dot = std::min(std::abs(Quaternion::dot(a, b)), 1.0F);
|
|
return std::acos(dot) * 2.0F;
|
|
}
|
|
|
|
void Quaternion::rotateTowards(const Quaternion &from, const Quaternion &to, float maxStep, Quaternion *dst) {
|
|
const auto angle = Quaternion::angle(from, to);
|
|
if (angle == 0.0F) {
|
|
dst->set(to);
|
|
return;
|
|
}
|
|
|
|
const auto t = std::min(maxStep / (angle * 180.F / math::PI), 1.0F);
|
|
Quaternion::slerp(from, to, t, dst);
|
|
}
|
|
|
|
void Quaternion::fromViewUp(const Vec3 &view, Quaternion *out) {
|
|
CC_ASSERT(out);
|
|
fromViewUp(view, Vec3(0, 1, 0), out);
|
|
}
|
|
void Quaternion::fromViewUp(const Vec3 &view, const Vec3 &up, Quaternion *out) {
|
|
CC_ASSERT(out);
|
|
Mat3 mTemp{Mat3::IDENTITY};
|
|
Mat3::fromViewUp(view, up, &mTemp);
|
|
Quaternion::fromMat3(mTemp, out);
|
|
out->normalize();
|
|
}
|
|
|
|
void Quaternion::fromEuler(float x, float y, float z, Quaternion *dst) {
|
|
CC_ASSERT(dst);
|
|
float halfToRad = 0.5F * cc::math::PI / 180.F;
|
|
x *= halfToRad;
|
|
y *= halfToRad;
|
|
z *= halfToRad;
|
|
float sx = std::sin(x);
|
|
float cx = std::cos(x);
|
|
float sy = std::sin(y);
|
|
float cy = std::cos(y);
|
|
float sz = std::sin(z);
|
|
float cz = std::cos(z);
|
|
|
|
dst->x = sx * cy * cz + cx * sy * sz;
|
|
dst->y = cx * sy * cz + sx * cy * sz;
|
|
dst->z = cx * cy * sz - sx * sy * cz;
|
|
dst->w = cx * cy * cz - sx * sy * sz;
|
|
}
|
|
|
|
void Quaternion::toEuler(const Quaternion &q, bool outerZ, Vec3 *out) {
|
|
CC_ASSERT(out);
|
|
float x{q.x};
|
|
float y{q.y};
|
|
float z{q.z};
|
|
float w{q.w};
|
|
float bank{0};
|
|
float heading{0};
|
|
float attitude{0};
|
|
float test = x * y + z * w;
|
|
float r2d = 180.F / math::PI;
|
|
if (test > 0.499999) {
|
|
bank = 0;
|
|
heading = 2 * atan2(x, w) * r2d;
|
|
attitude = 90;
|
|
} else if (test < -0.499999) {
|
|
bank = 0;
|
|
heading = -2 * atan2(x, w) * r2d;
|
|
attitude = -90;
|
|
} else {
|
|
float sqx = x * x;
|
|
float sqy = y * y;
|
|
float sqz = z * z;
|
|
bank = atan2(2 * x * w - 2 * y * z, 1 - 2 * sqx - 2 * sqz) * r2d;
|
|
heading = atan2(2 * y * w - 2 * x * z, 1 - 2 * sqy - 2 * sqz) * r2d;
|
|
attitude = asin(2 * test) * r2d;
|
|
if (outerZ) {
|
|
bank = static_cast<float>(-180.F * math::sgn(bank + 1e-6) + bank);
|
|
heading = static_cast<float>(-180.F * math::sgn(heading + 1e-6) + heading);
|
|
attitude = static_cast<float>(180.F * math::sgn(attitude + 1e-6) - attitude);
|
|
}
|
|
}
|
|
out->x = bank;
|
|
out->y = heading;
|
|
out->z = attitude;
|
|
}
|
|
|
|
void Quaternion::fromMat3(const Mat3 &m, Quaternion *out) {
|
|
CC_ASSERT(out);
|
|
float m00 = m.m[0];
|
|
float m01 = m.m[1];
|
|
float m02 = m.m[2];
|
|
float m10 = m.m[3];
|
|
float m11 = m.m[4];
|
|
float m12 = m.m[5];
|
|
float m20 = m.m[6];
|
|
float m21 = m.m[7];
|
|
float m22 = m.m[8];
|
|
|
|
float fourXSquaredMinus1 = m00 - m11 - m22;
|
|
float fourYSquaredMinus1 = m11 - m00 - m22;
|
|
float fourZSquaredMinus1 = m22 - m00 - m11;
|
|
float fourWSquaredMinus1 = m00 + m11 + m22;
|
|
|
|
int biggestIndex = 0;
|
|
float fourBiggestSquaredMinus1 = fourWSquaredMinus1;
|
|
if (fourXSquaredMinus1 > fourBiggestSquaredMinus1) {
|
|
fourBiggestSquaredMinus1 = fourXSquaredMinus1;
|
|
biggestIndex = 1;
|
|
}
|
|
if (fourYSquaredMinus1 > fourBiggestSquaredMinus1) {
|
|
fourBiggestSquaredMinus1 = fourYSquaredMinus1;
|
|
biggestIndex = 2;
|
|
}
|
|
if (fourZSquaredMinus1 > fourBiggestSquaredMinus1) {
|
|
fourBiggestSquaredMinus1 = fourZSquaredMinus1;
|
|
biggestIndex = 3;
|
|
}
|
|
|
|
float biggestVal = std::sqrt(fourBiggestSquaredMinus1 + 1) * 0.5F;
|
|
float mult = 0.25F / biggestVal;
|
|
switch (biggestIndex) {
|
|
case 0:
|
|
out->w = biggestVal;
|
|
out->x = (m12 - m21) * mult;
|
|
out->y = (m20 - m02) * mult;
|
|
out->z = (m01 - m10) * mult;
|
|
break;
|
|
case 1:
|
|
out->w = (m12 - m21) * mult;
|
|
out->x = biggestVal;
|
|
out->y = (m01 + m10) * mult;
|
|
out->z = (m20 + m02) * mult;
|
|
break;
|
|
case 2:
|
|
out->w = (m20 - m02) * mult;
|
|
out->x = (m01 + m10) * mult;
|
|
out->y = biggestVal;
|
|
out->z = (m12 + m21) * mult;
|
|
break;
|
|
case 3:
|
|
out->w = (m01 - m10) * mult;
|
|
out->x = (m20 + m02) * mult;
|
|
out->y = (m12 + m21) * mult;
|
|
out->z = biggestVal;
|
|
break;
|
|
default: // Silence a -Wswitch-default warning in GCC. Should never actually get here. Assert is just for sanity.
|
|
CC_ASSERT(false);
|
|
out->w = 1.F;
|
|
out->x = 0.F;
|
|
out->y = 0.F;
|
|
out->z = 0.F;
|
|
break;
|
|
}
|
|
}
|
|
|
|
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.
|
|
CC_ASSERT(dstx && dsty && dstz && dstw);
|
|
CC_ASSERT(!(t < 0.F || t > 1.F));
|
|
|
|
if (t == 0.F) {
|
|
*dstx = q1x;
|
|
*dsty = q1y;
|
|
*dstz = q1z;
|
|
*dstw = q1w;
|
|
return;
|
|
}
|
|
|
|
if (t == 1.F) {
|
|
*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;
|
|
float alpha;
|
|
float beta;
|
|
|
|
float u;
|
|
float f1;
|
|
float f2a;
|
|
float f2b;
|
|
|
|
float ratio1;
|
|
float ratio2;
|
|
|
|
float halfSecHalfTheta;
|
|
float versHalfTheta;
|
|
|
|
float sqNotU;
|
|
float sqU;
|
|
|
|
float cosTheta = q1w * q2w + q1x * q2x + q1y * q2y + q1z * q2z;
|
|
|
|
// As usual in all slerp implementations, we fold theta.
|
|
alpha = cosTheta >= 0 ? 1.F : -1.F;
|
|
halfY = 1.F + 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.F - 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.F - halfY * halfSecHalfTheta;
|
|
|
|
// Evaluate series expansions of the coefficients.
|
|
sqNotU = f1 * f1;
|
|
ratio2 = 0.0000440917108F * versHalfTheta;
|
|
ratio1 = -0.00158730159F + (sqNotU - 16.F) * ratio2;
|
|
ratio1 = 0.0333333333F + ratio1 * (sqNotU - 9.F) * versHalfTheta;
|
|
ratio1 = -0.333333333F + ratio1 * (sqNotU - 4.F) * versHalfTheta;
|
|
ratio1 = 1.F + ratio1 * (sqNotU - 1.F) * versHalfTheta;
|
|
|
|
sqU = u * u;
|
|
ratio2 = -0.00158730159F + (sqU - 16.F) * ratio2;
|
|
ratio2 = 0.0333333333F + ratio2 * (sqU - 9.F) * versHalfTheta;
|
|
ratio2 = -0.333333333F + ratio2 * (sqU - 4.F) * versHalfTheta;
|
|
ratio2 = 1.F + ratio2 * (sqU - 1.F) * 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) {
|
|
CC_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.F) {
|
|
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.F - 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);
|
|
}
|
|
|
|
NS_CC_MATH_END
|
|
|