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
|
/* ResidualVM - A 3D game interpreter
*
* ResidualVM is the legal property of its developers, whose names
* are too numerous to list here. Please refer to the COPYRIGHT
* file distributed with this source distribution.
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* as published by the Free Software Foundation; either version 2
* of the License, or (at your option) any later version.
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*
*/
// Quaternion-math borrowed from plib http://plib.sourceforge.net/index.html
// Which is covered by LGPL2
// And has this additional copyright note:
/*
Quaternion routines are Copyright (C) 1999
Kevin B. Thompson <kevinbthompson@yahoo.com>
Modified by Sylvan W. Clebsch <sylvan@stanford.edu>
Largely rewritten by "Negative0" <negative0@earthlink.net>
*/
#include "common/streamdebug.h"
#include "math/quat.h"
namespace Math {
Quaternion Quaternion::slerpQuat(const Quaternion& to, const float t) {
Quaternion dst;
float co, scale0, scale1;
bool flip = false ;
/* SWC - Interpolate between to quaternions */
co = this->dotProduct(to);
if (co < 0.0f) {
co = -co;
flip = true;
}
if ( co < 1.0f - (float) 1e-6 ) {
float o = (float) acos ( co );
float so = 1.0f / (float) sin ( o );
scale0 = (float) sin ( (1.0f - t) * o ) * so;
scale1 = (float) sin ( t * o ) * so;
} else {
scale0 = 1.0f - t;
scale1 = t;
}
if (flip) {
scale1 = -scale1 ;
}
dst.x() = scale0 * this->x() + scale1 * to.x() ;
dst.y() = scale0 * this->y() + scale1 * to.y() ;
dst.z() = scale0 * this->z() + scale1 * to.z() ;
dst.w() = scale0 * this->w() + scale1 * to.w() ;
return dst;
}
void Quaternion::toMatrix(Matrix4 &dst) {
float two_xx = x() * (x() + x());
float two_xy = x() * (y() + y());
float two_xz = x() * (z() + z());
float two_wx = w() * (x() + x());
float two_wy = w() * (y() + y());
float two_wz = w() * (z() + z());
float two_yy = y() * (y() + y());
float two_yz = y() * (z() + z());
float two_zz = z() * (z() + z());
float newMat[16] = {
1.0f-(two_yy+two_zz), two_xy-two_wz, two_xz+two_wy, 0.0f,
two_xy+two_wz, 1.0f-(two_xx+two_zz), two_yz-two_wx, 0.0f,
two_xz-two_wy, two_yz+two_wx, 1.0f-(two_xx+two_yy), 0.0f,
0.0f, 0.0f, 0.0f, 1.0f
};
dst.setData(newMat);
}
Matrix4 Quaternion::toMatrix() {
Matrix4 dst;
toMatrix(dst);
return dst;
}
}
|
