Open Broadcaster Software
Free, open source software for live streaming and recording
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
quat.h
Go to the documentation of this file.
1 /******************************************************************************
2  Copyright (C) 2013 by Hugh Bailey <obs.jim@gmail.com>
3 
4  This program is free software: you can redistribute it and/or modify
5  it under the terms of the GNU General Public License as published by
6  the Free Software Foundation, either version 2 of the License, or
7  (at your option) any later version.
8 
9  This program is distributed in the hope that it will be useful,
10  but WITHOUT ANY WARRANTY; without even the implied warranty of
11  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  GNU General Public License for more details.
13 
14  You should have received a copy of the GNU General Public License
15  along with this program. If not, see <http://www.gnu.org/licenses/>.
16 ******************************************************************************/
17 
18 #pragma once
19 
20 #include "../util/c99defs.h"
21 #include "math-defs.h"
22 #include "vec3.h"
23 #include <xmmintrin.h>
24 
25 /*
26  * Quaternion math
27  *
28  * Generally used to represent rotational data more than anything. Allows
29  * for efficient and correct rotational interpolation without suffering from
30  * things like gimbal lock.
31  */
32 
33 #ifdef __cplusplus
34 extern "C" {
35 #endif
36 
37 struct matrix3;
38 struct matrix4;
39 struct axisang;
40 
41 struct quat {
42  union {
43  struct {float x, y, z, w;};
44  float ptr[4];
45  __m128 m;
46  };
47 };
48 
49 static inline void quat_identity(struct quat *q)
50 {
51  q->m = _mm_setzero_ps();
52  q->w = 1.0f;
53 }
54 
55 static inline void quat_set(struct quat *dst, float x, float y, float z,
56  float w)
57 {
58  dst->m = _mm_set_ps(x, y, z, w);
59 }
60 
61 static inline void quat_copy(struct quat *dst, const struct quat *q)
62 {
63  dst->m = q->m;
64 }
65 
66 static inline void quat_add(struct quat *dst, const struct quat *q1,
67  const struct quat *q2)
68 {
69  dst->m = _mm_add_ps(q1->m, q2->m);
70 }
71 
72 static inline void quat_sub(struct quat *dst, const struct quat *q1,
73  const struct quat *q2)
74 {
75  dst->m = _mm_sub_ps(q1->m, q2->m);
76 }
77 
78 EXPORT void quat_mul(struct quat *dst, const struct quat *q1,
79  const struct quat *q2);
80 
81 static inline void quat_addf(struct quat *dst, const struct quat *q,
82  float f)
83 {
84  dst->m = _mm_add_ps(q->m, _mm_set1_ps(f));
85 }
86 
87 static inline void quat_subf(struct quat *dst, const struct quat *q,
88  float f)
89 {
90  dst->m = _mm_sub_ps(q->m, _mm_set1_ps(f));
91 }
92 
93 static inline void quat_mulf(struct quat *dst, const struct quat *q,
94  float f)
95 {
96  dst->m = _mm_mul_ps(q->m, _mm_set1_ps(f));
97 }
98 
99 static inline void quat_divf(struct quat *dst, const struct quat *q,
100  float f)
101 {
102  dst->m = _mm_div_ps(q->m, _mm_set1_ps(f));
103 }
104 
105 static inline float quat_dot(const struct quat *q1, const struct quat *q2)
106 {
107  struct vec3 add;
108  __m128 mul = _mm_mul_ps(q1->m, q2->m);
109  add.m = _mm_add_ps(_mm_movehl_ps(mul, mul), mul);
110  add.m = _mm_add_ps(_mm_shuffle_ps(add.m, add.m, 0x55), add.m);
111  return add.x;
112 }
113 
114 static inline void quat_inv(struct quat *dst, const struct quat *q)
115 {
116  dst->x = -q->x;
117  dst->y = -q->y;
118  dst->z = -q->z;
119 }
120 
121 static inline void quat_neg(struct quat *dst, const struct quat *q)
122 {
123  dst->x = -q->x;
124  dst->y = -q->y;
125  dst->z = -q->z;
126  dst->w = -q->w;
127 }
128 
129 static inline float quat_len(const struct quat *q)
130 {
131  float dot_val = quat_dot(q, q);
132  return (dot_val > 0.0f) ? sqrtf(dot_val) : 0.0f;
133 }
134 
135 static inline float quat_dist(const struct quat *q1, const struct quat *q2)
136 {
137  struct quat temp;
138  float dot_val;
139 
140  quat_sub(&temp, q1, q2);
141  dot_val = quat_dot(&temp, &temp);
142  return (dot_val > 0.0f) ? sqrtf(dot_val) : 0.0f;
143 }
144 
145 static inline void quat_norm(struct quat *dst, const struct quat *q)
146 {
147  float dot_val = quat_dot(q, q);
148  dst->m = (dot_val > 0.0f) ?
149  _mm_mul_ps(q->m, _mm_set1_ps(1.0f/sqrtf(dot_val))) :
150  _mm_setzero_ps();
151 }
152 
153 static inline bool quat_close(const struct quat *q1, const struct quat *q2,
154  float epsilon)
155 {
156  struct quat test;
157  quat_sub(&test, q1, q2);
158  return test.x < epsilon &&
159  test.y < epsilon &&
160  test.z < epsilon &&
161  test.w < epsilon;
162 }
163 
164 EXPORT void quat_from_axisang(struct quat *dst, const struct axisang *aa);
165 EXPORT void quat_from_matrix3(struct quat *dst, const struct matrix3 *m);
166 EXPORT void quat_from_matrix4(struct quat *dst, const struct matrix4 *m);
167 
168 EXPORT void quat_get_dir(struct vec3 *dst, const struct quat *q);
169 EXPORT void quat_set_look_dir(struct quat *dst, const struct vec3 *dir);
170 
171 EXPORT void quat_log(struct quat *dst, const struct quat *q);
172 EXPORT void quat_exp(struct quat *dst, const struct quat *q);
173 
174 EXPORT void quat_interpolate(struct quat *dst, const struct quat *q1,
175  const struct quat *q2, float t);
176 EXPORT void quat_get_tangent(struct quat *dst, const struct quat *prev,
177  const struct quat *q, const struct quat *next);
178 EXPORT void quat_interpolate_cubic(struct quat *dst, const struct quat *q1,
179  const struct quat *q2, const struct quat *m1,
180  const struct quat *m2, float t);
181 
182 #ifdef __cplusplus
183 }
184 #endif
float ptr[4]
Definition: quat.h:44
EXPORT void quat_interpolate_cubic(struct quat *dst, const struct quat *q1, const struct quat *q2, const struct quat *m1, const struct quat *m2, float t)
Definition: axisang.h:28
EXPORT void quat_set_look_dir(struct quat *dst, const struct vec3 *dir)
Definition: vec3.h:33
EXPORT void quat_get_dir(struct vec3 *dst, const struct quat *q)
EXPORT void quat_mul(struct quat *dst, const struct quat *q1, const struct quat *q2)
EXPORT void quat_exp(struct quat *dst, const struct quat *q)
EXPORT void quat_get_tangent(struct quat *dst, const struct quat *prev, const struct quat *q, const struct quat *next)
Definition: matrix3.h:31
#define EXPORT
Definition: c99defs.h:49
float y
Definition: quat.h:43
EXPORT void quat_from_matrix4(struct quat *dst, const struct matrix4 *m)
EXPORT void quat_log(struct quat *dst, const struct quat *q)
float w
Definition: quat.h:43
float x
Definition: quat.h:43
Definition: matrix4.h:32
EXPORT void quat_from_matrix3(struct quat *dst, const struct matrix3 *m)
EXPORT void quat_interpolate(struct quat *dst, const struct quat *q1, const struct quat *q2, float t)
Definition: quat.h:41
float z
Definition: quat.h:43
__m128 m
Definition: quat.h:45
EXPORT void quat_from_axisang(struct quat *dst, const struct axisang *aa)