1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
|
#include "m/m_quat.h"
#include "c/c_math.h"
#include "common.h"
#include "egg/math/eggMath.h"
#include "m/m_vec.h"
#include "nw4r/math/math_arithmetic.h"
#include "nw4r/ut/ut_algorithm.h"
bool mQuat_c::Set(const mVec3_c &from, const mVec3_c &to) {
mVec3_c cross;
cross.x = (from.y * to.z) - (from.z * to.y);
cross.y = (from.z * to.x) - (from.x * to.z);
cross.z = (from.x * to.y) - (from.y * to.x);
f32 dot = (from.dot(to) + 1.0f) * 2.0f;
if (dot < 0.0f) {
dot = 0.0f;
}
f32 f = nw4r::math::FSqrt(dot);
if (f <= EGG::Math<f32>::epsilon()) {
return false;
}
f32 norm = (1.0f / f);
set(f * 0.5f, cross.x * norm, cross.y * norm, cross.z * norm);
return true;
}
bool mQuat_c::slerp(const mVec3_c &from, const mVec3_c &to, f32 interp) {
f32 z = (from.x * to.y) - (from.y * to.x);
f32 y = (from.z * to.x) - (from.x * to.z);
f32 x = (from.y * to.z) - (from.z * to.y);
mVec3_c cross(x, y, z);
f32 mag = cross.mag();
if (cM::isZero(mag)) {
set(1.0f, 0.0f, 0.0f, 0.0f);
return from.dot(to) > 0.0f;
}
cross *= 1.0f / mag;
setAxisRotation(cross, interp * cM::atan2f(mag, from.dot(to)));
return true;
}
bool mQuat_c::slerpTo(const mVec3_c &from, const mVec3_c &to, f32 maxStep) {
f32 z = (from.x * to.y) - (from.y * to.x);
f32 y = (from.z * to.x) - (from.x * to.z);
f32 x = (from.y * to.z) - (from.z * to.y);
mVec3_c cross(x, y, z);
f32 mag = cross.mag();
if (cM::isZero(mag)) {
set(1.0f, 0.0f, 0.0f, 0.0f);
return from.dot(to) > 0.0f;
}
cross *= 1.0f / mag;
setAxisRotation(cross, nw4r::ut::Min(cM::atan2f(mag, from.dot(to)), maxStep));
return true;
}
|
