framework2

1
#include "ofQuaternion.h"
2
#include "ofMatrix4x4.h"
3
#include "ofMath.h"
4
#include "ofMathConstants.h"
5

6
#define GLM_FORCE_CTOR_INIT
7
#include "glm/gtc/quaternion.hpp"
8

9
//----------------------------------------
10
ofQuaternion::ofQuaternion(const glm::quat & q):_v(q.x, q.y, q.z, q.w){}
11

12
//----------------------------------------
13
ofQuaternion::operator glm::quat() const{
14
	return glm::quat(_v.w, glm::vec3(_v.x, _v.y, _v.z));
15
}
16

17
void ofQuaternion::set(const ofMatrix4x4& matrix) {
18
	*this = matrix.getRotate();
19
}
20

21
void ofQuaternion::get(ofMatrix4x4& matrix) const {
22
	matrix.makeRotationMatrix(*this);
23
}
24

25

26
/// Set the elements of the Quat to represent a rotation of angle
27
/// (degrees) around the axis (x,y,z)
28
void ofQuaternion::makeRotate( float angle, float x, float y, float z ) {
29
	angle = ofDegToRad(angle);
30
	
31
	// FIXME: why not using std::numeric_limits<float>::epsilon() ?
32
	const float epsilon = 0.0000001f;
33

34
	float length = sqrtf( x * x + y * y + z * z );
35
	if (length < epsilon) {
36
		// ~zero length axis, so reset rotation to zero.
37
		*this = ofQuaternion();
38
		return;
39
	}
40

41
	float inversenorm  = 1.0f / length;
42
	float coshalfangle = cosf( 0.5f * angle );
43
	float sinhalfangle = sinf( 0.5f * angle );
44

45
	_v.x = x * sinhalfangle * inversenorm;
46
	_v.y = y * sinhalfangle * inversenorm;
47
	_v.z = z * sinhalfangle * inversenorm;
48
	_v.w = coshalfangle;
49
}
50

51

52
void ofQuaternion::makeRotate( float angle, const ofVec3f& vec ) {
53
	makeRotate( angle, vec.x, vec.y, vec.z );
54
}
55

56

57
void ofQuaternion::makeRotate ( float angle1, const ofVec3f& axis1,
58
                          float angle2, const ofVec3f& axis2,
59
                          float angle3, const ofVec3f& axis3) {
60
       ofQuaternion q1; q1.makeRotate(angle1,axis1);
61
       ofQuaternion q2; q2.makeRotate(angle2,axis2);
62
       ofQuaternion q3; q3.makeRotate(angle3,axis3);
63

64
       *this = q1*q2*q3;
65
}
66

67
/** Make a rotation Quat which will rotate vec1 to vec2
68

69
 This routine uses only fast geometric transforms, without costly acos/sin computations.
70
 It's exact, fast, and with less degenerate cases than the acos/sin method.
71

72
 For an explanation of the math used, you may see for example:
73
 http://logiciels.cnes.fr/MARMOTTES/marmottes-mathematique.pdf
74

75
 @note This is the rotation with shortest angle, which is the one equivalent to the
76
 acos/sin transform method. Other rotations exists, for example to additionally keep
77
 a local horizontal attitude.
78

79
 @author Nicolas Brodu
80
 */
81
void ofQuaternion::makeRotate( const ofVec3f& from, const ofVec3f& to ) {
82

83
	// This routine takes any vector as argument but normalized
84
	// vectors are necessary, if only for computing the dot product.
85
	// Too bad the API is that generic, it leads to performance loss.
86
	// Even in the case the 2 vectors are not normalized but same length,
87
	// the sqrt could be shared, but we have no way to know beforehand
88
	// at this point, while the caller may know.
89
	// So, we have to test... in the hope of saving at least a sqrt
90
	ofVec3f sourceVector = from;
91
	ofVec3f targetVector = to;
92

93
	float fromLen2 = from.lengthSquared();
94
	float fromLen;
95
	// normalize only when necessary, epsilon test
96
	if ((fromLen2 < 1.0 - 1e-7) || (fromLen2 > 1.0 + 1e-7)) {
97
		fromLen = sqrt(fromLen2);
98
		sourceVector /= fromLen;
99
	} else fromLen = 1.0;
100

101
	float toLen2 = to.lengthSquared();
102
	// normalize only when necessary, epsilon test
103
	if ((toLen2 < 1.0 - 1e-7) || (toLen2 > 1.0 + 1e-7)) {
104
		float toLen;
105
		// re-use fromLen for case of mapping 2 vectors of the same length
106
		if ((toLen2 > fromLen2 - 1e-7) && (toLen2 < fromLen2 + 1e-7)) {
107
			toLen = fromLen;
108
		} else toLen = sqrt(toLen2);
109
		targetVector /= toLen;
110
	}
111

112

113
	// Now let's get into the real stuff
114
	// Use "dot product plus one" as test as it can be re-used later on
115
	double dotProdPlus1 = 1.0 + sourceVector.dot(targetVector);
116

117
	// Check for degenerate case of full u-turn. Use epsilon for detection
118
	if (dotProdPlus1 < 1e-7) {
119

120
		// Get an orthogonal vector of the given vector
121
		// in a plane with maximum vector coordinates.
122
		// Then use it as quaternion axis with pi angle
123
		// Trick is to realize one value at least is >0.6 for a normalized vector.
124
		if (fabs(sourceVector.x) < 0.6) {
125
			const double norm = sqrt(1.0 - sourceVector.x * sourceVector.x);
126
			_v.x = 0.0;
127
			_v.y = sourceVector.z / norm;
128
			_v.z = -sourceVector.y / norm;
129
			_v.w = 0.0;
130
		} else if (fabs(sourceVector.y) < 0.6) {
131
			const double norm = sqrt(1.0 - sourceVector.y * sourceVector.y);
132
			_v.x = -sourceVector.z / norm;
133
			_v.y = 0.0;
134
			_v.z = sourceVector.x / norm;
135
			_v.w = 0.0;
136
		} else {
137
			const double norm = sqrt(1.0 - sourceVector.z * sourceVector.z);
138
			_v.x = sourceVector.y / norm;
139
			_v.y = -sourceVector.x / norm;
140
			_v.z = 0.0;
141
			_v.w = 0.0;
142
		}
143
	}
144

145
	else {
146
		// Find the shortest angle quaternion that transforms normalized vectors
147
		// into one other. Formula is still valid when vectors are colinear
148
		const double s = sqrt(0.5 * dotProdPlus1);
149
		const ofVec3f tmp = sourceVector.getCrossed(targetVector) / (2.0 * s);
150
		_v.x = tmp.x;
151
		_v.y = tmp.y;
152
		_v.z = tmp.z;
153
		_v.w = s;
154
	}
155
}
156

157

158
// Make a rotation Quat which will rotate vec1 to vec2
159
// Generally take adot product to get the angle between these
160
// and then use a cross product to get the rotation axis
161
// Watch out for the two special cases of when the vectors
162
// are co-incident or opposite in direction.
163
void ofQuaternion::makeRotate_original( const ofVec3f& from, const ofVec3f& to ) {
164
	const float epsilon = 0.0000001f;
165

166
	float length1  = from.length();
167
	float length2  = to.length();
168

169
	// dot product vec1*vec2
170
	float cosangle = from.dot(to) / (length1 * length2);
171

172
	if ( fabs(cosangle - 1) < epsilon ) {
173
		//osg::notify(osg::INFO)<<"*** Quat::makeRotate(from,to) with near co-linear vectors, epsilon= "<<fabs(cosangle-1)<<std::endl;
174

175
		// cosangle is close to 1, so the vectors are close to being coincident
176
		// Need to generate an angle of zero with any vector we like
177
		// We'll choose (1,0,0)
178
		makeRotate( 0.0, 0.0, 0.0, 1.0 );
179
	} else
180
		if ( fabs(cosangle + 1.0) < epsilon ) {
181
			// vectors are close to being opposite, so will need to find a
182
			// vector orthongonal to from to rotate about.
183
			ofVec3f tmp;
184
			if (fabs(from.x) < fabs(from.y))
185
				if (fabs(from.x) < fabs(from.z)) tmp.set(1.0, 0.0, 0.0); // use x axis.
186
				else tmp.set(0.0, 0.0, 1.0);
187
			else if (fabs(from.y) < fabs(from.z)) tmp.set(0.0, 1.0, 0.0);
188
			else tmp.set(0.0, 0.0, 1.0);
189

190
			ofVec3f fromd(from.x, from.y, from.z);
191

192
			// find orthogonal axis.
193
			ofVec3f axis(fromd.getCrossed(tmp));
194
			axis.normalize();
195

196
			_v.x = axis[0]; // sine of half angle of PI is 1.0.
197
			_v.y = axis[1]; // sine of half angle of PI is 1.0.
198
			_v.z = axis[2]; // sine of half angle of PI is 1.0.
199
			_v.w = 0; // cosine of half angle of PI is zero.
200

201
		} else {
202
			// This is the usual situation - take a cross-product of vec1 and vec2
203
			// and that is the axis around which to rotate.
204
			ofVec3f axis(from.getCrossed(to));
205
			float angle = acos( cosangle );
206
			makeRotate( angle, axis );
207
		}
208
}
209

210
void ofQuaternion::getRotate( float& angle, ofVec3f& vec ) const {
211
	float x, y, z;
212
	getRotate(angle, x, y, z);
213
	vec.x = x;
214
	vec.y = y;
215
	vec.z = z;
216
}
217
// Get the angle of rotation and axis of this Quat object.
218
// Won't give very meaningful results if the Quat is not associated
219
// with a rotation!
220
void ofQuaternion::getRotate( float& angle, float& x, float& y, float& z ) const {
221
	float sinhalfangle = sqrt( _v.x * _v.x + _v.y * _v.y + _v.z * _v.z );
222

223
	angle = 2.0 * atan2( sinhalfangle, _v.w );
224
	if (sinhalfangle) {
225
		x = _v.x / sinhalfangle;
226
		y = _v.y / sinhalfangle;
227
		z = _v.z / sinhalfangle;
228
	} else {
229
		x = 0.0;
230
		y = 0.0;
231
		z = 1.0;
232
	}
233
	
234
	angle = ofRadToDeg(angle);
235
}
236

237

238
/// Spherical Linear Interpolation
239
/// As t goes from 0 to 1, the Quat object goes from "from" to "to"
240
/// Reference: Shoemake at SIGGRAPH 89
241
/// See also
242
/// http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
243
void ofQuaternion::slerp( float t, const ofQuaternion& from, const ofQuaternion& to ) {
244
	
245
	// FIXME: std::numeric_limits<double>::epsilon()
246
	const double epsilon = 0.00001;
247
	double omega, cosomega, sinomega, scale_from, scale_to ;
248

249
	ofQuaternion quatTo(to);
250
	// this is a dot product
251

252
	cosomega = from.asVec4().dot(to.asVec4());
253

254
	if ( cosomega < 0.0 ) {
255
		cosomega = -cosomega;
256
		quatTo = -to;
257
	}
258

259
	if ( (1.0 - cosomega) > epsilon ) {
260
		omega = acos(cosomega) ; // 0 <= omega <= Pi (see man acos)
261
		sinomega = sin(omega) ;  // this sinomega should always be +ve so
262
		// could try sinomega=sqrt(1-cosomega*cosomega) to avoid a sin()?
263
		scale_from = sin((1.0 - t) * omega) / sinomega ;
264
		scale_to = sin(t * omega) / sinomega ;
265
	} else {
266
		/* --------------------------------------------------
267
		The ends of the vectors are very close
268
		we can use simple linear interpolation - no need
269
		to worry about the "spherical" interpolation
270
		-------------------------------------------------- */
271
		scale_from = 1.0 - t ;
272
		scale_to = t ;
273
	}
274

275
	*this = (from * scale_from) + (quatTo * scale_to);
276

277
	// so that we get a Vec4
278
}
279

280

281
// ref at http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
282
ofVec3f ofQuaternion::getEuler() const {
283
	float test = x()*y() + z()*w();
284
	float heading;
285
	float attitude;
286
	float bank;
287
	if (test > 0.499) { // singularity at north pole
288
		heading = 2.0f * atan2(x(), w());
289
		attitude = glm::half_pi<float>();
290
		bank = 0;
291
	} else if (test < -0.499) { // singularity at south pole
292
		heading = -2.0f * atan2(x(), w());
293
		attitude = - glm::half_pi<float>();
294
		bank = 0;
295
	} else {
296
		float sqx = x() * x();
297
		float sqy = y() * y();
298
		float sqz = z() * z();
299
		heading = atan2(2.0f * y() * w() - 2.0f * x() * z(), 1.0f - 2.0f*sqy - 2.0f*sqz);
300
		attitude = asin(2*test);
301
		bank = atan2(2.0f*x() * w() - 2.0f * y() * z(), 1.0f - 2.0f*sqx - 2.0f*sqz);
302
	}
303
	
304
	return ofVec3f(ofRadToDeg(bank), ofRadToDeg(heading), ofRadToDeg(attitude));
305
}
306

307
#define QX  _v.x
308
#define QY  _v.y
309
#define QZ  _v.z
310
#define QW  _v.w
311

312
//----------------------------------------
313
std::ostream& operator<<(std::ostream& os, const ofQuaternion &q) {
314
    os << q._v.x << ", " << q._v.y << ", " << q._v.z << ", " << q._v.w;
315
    return os;
316
}
317

318

319
//----------------------------------------
320
std::istream& operator>>(std::istream& is, ofQuaternion &q) {
321
    is >> q._v.x;
322
    is.ignore(2);
323
    is >> q._v.y;
324
    is.ignore(2);
325
    is >> q._v.z;
326
    is.ignore(2);
327
    is >> q._v.w;
328
    return is;
329
}
330