Open Broadcaster Software
Free, open source software for live streaming and recording
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:53
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
__m128 m
Definition: vec3.h:39
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 x
Definition: vec3.h:36
float z
Definition: quat.h:43
__m128 m
Definition: quat.h:45
EXPORT void quat_from_axisang(struct quat *dst, const struct axisang *aa)