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
182
183
184
185
186
187
188
|
/*
* Cogl
*
* An object oriented GL/GLES Abstraction/Utility Layer
*
* Copyright (C) 2010 Intel Corporation.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2 of the License, or (at your option) any later version.
*
* This library 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
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the
* Free Software Foundation, Inc., 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
* Authors:
* Robert Bragg <robert@linux.intel.com>
*/
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif
#include <cogl-util.h>
#include <cogl-euler.h>
#include <cogl-matrix.h>
#include <math.h>
#include <string.h>
void
cogl_euler_init (CoglEuler *euler,
float heading,
float pitch,
float roll)
{
euler->heading = heading;
euler->pitch = pitch;
euler->roll = roll;
}
void
cogl_euler_init_from_matrix (CoglEuler *euler,
const CoglMatrix *matrix)
{
/*
* Extracting a canonical Euler angle from a matrix:
* (where it is assumed the matrix contains no scaling, mirroring or
* skewing)
*
* A Euler angle is a combination of three rotations around mutually
* perpendicular axis. For this algorithm they are:
*
* Heading: A rotation about the Y axis by an angle H:
* | cosH 0 sinH|
* | 0 1 0|
* |-sinH 0 cosH|
*
* Pitch: A rotation around the X axis by an angle P:
* |1 0 0|
* |0 cosP -sinP|
* |0 sinP cosP|
*
* Roll: A rotation about the Z axis by an angle R:
* |cosR -sinR 0|
* |sinR cosR 0|
* | 0 0 1|
*
* When multiplied as matrices this gives:
* | cosHcosR+sinHsinPsinR sinRcosP -sinHcosR+cosHsinPsinR|
* M = |-cosHsinR+sinHsinPcosR cosRcosP sinRsinH+cosHsinPcosB|
* | sinHcosP -sinP cosHcosP |
*
* Given that there are an infinite number of ways to represent
* a given orientation, the "canonical" Euler angle is any such that:
* -180 < H < 180,
* -180 < R < 180 and
* -90 < P < 90
*
* M[3][2] = -sinP lets us immediately solve for P = asin(-M[3][2])
* (Note: asin has a range of +-90)
* This gives cosP
* This means we can use M[3][1] to calculate sinH:
* sinH = M[3][1]/cosP
* And use M[3][3] to calculate cosH:
* cosH = M[3][3]/cosP
* This lets us calculate H = atan2(sinH,cosH), but we optimise this:
* 1st note: atan2(x, y) does: atan(x/y) and uses the sign of x and y to
* determine the quadrant of the final angle.
* 2nd note: we know cosP is > 0 (ignoring cosP == 0)
* Therefore H = atan2((M[3][1]/cosP) / (M[3][3]/cosP)) can be simplified
* by skipping the division by cosP since it won't change the x/y ratio
* nor will it change their sign. This gives:
* H = atan2(M[3][1], M[3][3])
* R is computed in the same way as H from M[1][2] and M[2][2] so:
* R = atan2(M[1][2], M[2][2])
* Note: If cosP were == 0 then H and R could not be calculated as above
* because all the necessary matrix values would == 0. In other words we are
* pitched vertically and so H and R would now effectively rotate around the
* same axis - known as "Gimbal lock". In this situation we will set all the
* rotation on H and set R = 0.
* So with P = R = 0 we have cosP = 0, sinR = 0 and cosR = 1
* We can substitute those into the above equation for M giving:
* | cosH 0 -sinH|
* |sinHsinP 0 cosHsinP|
* | 0 -sinP 0|
* And calculate H as atan2 (-M[3][2], M[1][1])
*/
float sinP;
float H; /* heading */
float P; /* pitch */
float R; /* roll */
/* NB: CoglMatrix provides struct members named according to the
* [row][column] indexed. So matrix->zx is row 3 column 1. */
sinP = -matrix->zy;
/* Determine the Pitch, avoiding domain errors with asin () which
* might occur due to previous imprecision in manipulating the
* matrix. */
if (sinP <= -1.0f)
P = -G_PI_2;
else if (sinP >= 1.0f)
P = G_PI_2;
else
P = asinf (sinP);
/* If P is too close to 0 then we have hit Gimbal lock */
if (sinP > 0.999f)
{
H = atan2f (-matrix->zy, matrix->xx);
R = 0;
}
else
{
H = atan2f (matrix->zx, matrix->zz);
R = atan2f (matrix->xy, matrix->yy);
}
euler->heading = H;
euler->pitch = P;
euler->roll = R;
}
CoglBool
cogl_euler_equal (const void *v1, const void *v2)
{
const CoglEuler *a = v1;
const CoglEuler *b = v2;
_COGL_RETURN_VAL_IF_FAIL (v1 != NULL, FALSE);
_COGL_RETURN_VAL_IF_FAIL (v2 != NULL, FALSE);
if (v1 == v2)
return TRUE;
return (a->heading == b->heading &&
a->pitch == b->pitch &&
a->roll == b->roll);
}
CoglEuler *
cogl_euler_copy (const CoglEuler *src)
{
if (G_LIKELY (src))
{
CoglEuler *new = g_slice_new (CoglEuler);
memcpy (new, src, sizeof (float) * 3);
return new;
}
else
return NULL;
}
void
cogl_euler_free (CoglEuler *euler)
{
g_slice_free (CoglEuler, euler);
}
|
