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
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
|
/****************************************************************************
**
** Copyright (C) 2008-2012 NVIDIA Corporation.
** Copyright (C) 2017 The Qt Company Ltd.
** Contact: https://www.qt.io/licensing/
**
** This file is part of Qt 3D Studio.
**
** $QT_BEGIN_LICENSE:GPL$
** Commercial License Usage
** Licensees holding valid commercial Qt licenses may use this file in
** accordance with the commercial license agreement provided with the
** Software or, alternatively, in accordance with the terms contained in
** a written agreement between you and The Qt Company. For licensing terms
** and conditions see https://www.qt.io/terms-conditions. For further
** information use the contact form at https://www.qt.io/contact-us.
**
** GNU General Public License Usage
** Alternatively, this file may be used under the terms of the GNU
** General Public License version 3 or (at your option) any later version
** approved by the KDE Free Qt Foundation. The licenses are as published by
** the Free Software Foundation and appearing in the file LICENSE.GPL3
** included in the packaging of this file. Please review the following
** information to ensure the GNU General Public License requirements will
** be met: https://www.gnu.org/licenses/gpl-3.0.html.
**
** $QT_END_LICENSE$
**
****************************************************************************/
#include "foundation/Qt3DSMathUtils.h"
#include "foundation/Qt3DSUtilities.h"
#include "foundation/Qt3DSMat33.h"
using namespace qt3ds;
using namespace qt3ds::foundation;
using namespace qt3ds::intrinsics;
QT3DSQuat qt3ds::foundation::computeQuatFromNormal(const QT3DSVec3 &n)
{
// parallel or anti-parallel
if (n.x > 0.9999f) {
// parallel
return QT3DSQuat::createIdentity();
} else if (n.x < -0.9999f) {
// anti-parallel
// contactQuaternion.fromAngleAxisFast(PXD_PI, Vector3(0.0f, 1.0f, 0.0f));
return QT3DSQuat(0.0f, 1.0f, 0.0f, 0.0f);
} else {
QT3DSVec3 rotVec(0.0f, -n.z, n.y);
// Convert to quat
NVReal angle = rotVec.magnitude();
rotVec *= 1.0f / angle;
// if(angle > 1.0f) angle = 1.0f;
angle = selectMin(angle, 1.0f);
// djs: injudiciously imbecilic use of trig functions, good thing Adam is going to trample
// this path like a
// frustrated rhinoceros in mating season
angle = NVAsin(angle);
// if(n.x < 0)
// angle = NVPi - angle;
angle = fsel(n.x, angle, NVPi - angle);
return QT3DSQuat(angle, rotVec);
}
}
/**
\brief computes a oriented bounding box around the scaled basis.
\param basis Input = skewed basis, Output = (normalized) orthogonal basis.
\return Bounding box extent.
*/
QT3DSVec3 qt3ds::foundation::optimizeBoundingBox(QT3DSMat33 &basis)
{
QT3DSVec3 *QT3DS_RESTRICT vec = &basis[0]; // PT: don't copy vectors if not needed...
// PT: since we store the magnitudes to memory, we can avoid the FCMNV afterwards
QT3DSVec3 magnitude(vec[0].magnitudeSquared(), vec[1].magnitudeSquared(),
vec[2].magnitudeSquared());
// find indices sorted by magnitude
#ifdef QT3DS_X360
int i = (QT3DSU32 &)(magnitude[1]) > (QT3DSU32 &)(magnitude[0]) ? 1 : 0;
int j = (QT3DSU32 &)(magnitude[2]) > (QT3DSU32 &)(magnitude[1 - i]) ? 2 : 1 - i;
#else
int i = magnitude[1] > magnitude[0] ? 1 : 0;
int j = magnitude[2] > magnitude[1 - i] ? 2 : 1 - i;
#endif
const int k = 3 - i - j;
#ifdef QT3DS_X360
if ((QT3DSU32 &)(magnitude[i]) < (QT3DSU32 &)(magnitude[j]))
#else
if (magnitude[i] < magnitude[j])
#endif
swap(i, j);
// ortho-normalize basis
NVReal invSqrt = NVRecipSqrt(magnitude[i]);
magnitude[i] *= invSqrt;
vec[i] *= invSqrt; // normalize the first axis
NVReal dotij = vec[i].dot(vec[j]);
NVReal dotik = vec[i].dot(vec[k]);
magnitude[i] += NVAbs(dotij) + NVAbs(dotik); // elongate the axis by projection of the other two
vec[j] -= vec[i] * dotij; // orthogonize the two remaining axii relative to vec[i]
vec[k] -= vec[i] * dotik;
magnitude[j] = vec[j].normalize();
NVReal dotjk = vec[j].dot(vec[k]);
magnitude[j] += NVAbs(dotjk); // elongate the axis by projection of the other one
vec[k] -= vec[j] * dotjk; // orthogonize vec[k] relative to vec[j]
magnitude[k] = vec[k].normalize();
return magnitude;
}
QT3DSQuat qt3ds::foundation::slerp(const NVReal t, const QT3DSQuat &left, const QT3DSQuat &right)
{
const NVReal quatEpsilon = (NVReal(1.0e-8f));
NVReal cosine = left.dot(right);
NVReal sign = NVReal(1);
if (cosine < 0) {
cosine = -cosine;
sign = NVReal(-1);
}
NVReal sine = NVReal(1) - cosine * cosine;
if (sine >= quatEpsilon * quatEpsilon) {
sine = NVSqrt(sine);
const NVReal angle = NVAtan2(sine, cosine);
const NVReal i_sin_angle = NVReal(1) / sine;
const NVReal leftw = NVSin(angle * (NVReal(1) - t)) * i_sin_angle;
const NVReal rightw = NVSin(angle * t) * i_sin_angle * sign;
return left * leftw + right * rightw;
}
return left;
}
void qt3ds::foundation::integrateTransform(const NVTransform &curTrans, const QT3DSVec3 &linvel,
const QT3DSVec3 &angvel, NVReal timeStep, NVTransform &result)
{
result.p = curTrans.p + linvel * timeStep;
// from void NVsDynamicsContext::integrateAtomPose(NVsRigidBody* atom, Cm::BitMap
// &shapeChangedMap) const:
// Integrate the rotation using closed form quaternion integrator
NVReal w = angvel.magnitudeSquared();
if (w != 0.0f) {
w = NVSqrt(w);
if (w != 0.0f) {
const NVReal v = timeStep * w * 0.5f;
const NVReal q = NVCos(v);
const NVReal s = NVSin(v) / w;
const QT3DSVec3 pqr = angvel * s;
const QT3DSQuat quatVel(pqr.x, pqr.y, pqr.z, 0);
QT3DSQuat out; // need to have temporary, otherwise we may overwrite input if &curTrans ==
// &result.
out = quatVel * curTrans.q;
out.x += curTrans.q.x * q;
out.y += curTrans.q.y * q;
out.z += curTrans.q.z * q;
out.w += curTrans.q.w * q;
result.q = out;
return;
}
}
// orientation stays the same - convert from quat to matrix:
result.q = curTrans.q;
}
|
