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 
24 #include "../util/sse-intrin.h"
25 
26 /*
27  * Quaternion math
28  *
29  * Generally used to represent rotational data more than anything. Allows
30  * for efficient and correct rotational interpolation without suffering from
31  * things like gimbal lock.
32  */
33 
34 #ifdef __cplusplus
35 extern "C" {
36 #endif
37 
38 struct matrix3;
39 struct matrix4;
40 struct axisang;
41 
42 struct quat {
43  union {
44  struct {
45  float x, y, z, w;
46  };
47  float ptr[4];
48  __m128 m;
49  };
50 };
51 
52 static inline void quat_identity(struct quat *q)
53 {
54  q->m = _mm_setzero_ps();
55  q->w = 1.0f;
56 }
57 
58 static inline void quat_set(struct quat *dst, float x, float y, float z,
59  float w)
60 {
61  dst->m = _mm_set_ps(x, y, z, w);
62 }
63 
64 static inline void quat_copy(struct quat *dst, const struct quat *q)
65 {
66  dst->m = q->m;
67 }
68 
69 static inline void quat_add(struct quat *dst, const struct quat *q1,
70  const struct quat *q2)
71 {
72  dst->m = _mm_add_ps(q1->m, q2->m);
73 }
74 
75 static inline void quat_sub(struct quat *dst, const struct quat *q1,
76  const struct quat *q2)
77 {
78  dst->m = _mm_sub_ps(q1->m, q2->m);
79 }
80 
81 EXPORT void quat_mul(struct quat *dst, const struct quat *q1,
82  const struct quat *q2);
83 
84 static inline void quat_addf(struct quat *dst, const struct quat *q, float f)
85 {
86  dst->m = _mm_add_ps(q->m, _mm_set1_ps(f));
87 }
88 
89 static inline void quat_subf(struct quat *dst, const struct quat *q, float f)
90 {
91  dst->m = _mm_sub_ps(q->m, _mm_set1_ps(f));
92 }
93 
94 static inline void quat_mulf(struct quat *dst, const struct quat *q, 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, float f)
100 {
101  dst->m = _mm_div_ps(q->m, _mm_set1_ps(f));
102 }
103 
104 static inline float quat_dot(const struct quat *q1, const struct quat *q2)
105 {
106  struct vec3 add;
107  __m128 mul = _mm_mul_ps(q1->m, q2->m);
108  add.m = _mm_add_ps(_mm_movehl_ps(mul, mul), mul);
109  add.m = _mm_add_ps(_mm_shuffle_ps(add.m, add.m, 0x55), add.m);
110  return add.x;
111 }
112 
113 static inline void quat_inv(struct quat *dst, const struct quat *q)
114 {
115  dst->x = -q->x;
116  dst->y = -q->y;
117  dst->z = -q->z;
118 }
119 
120 static inline void quat_neg(struct quat *dst, const struct quat *q)
121 {
122  dst->x = -q->x;
123  dst->y = -q->y;
124  dst->z = -q->z;
125  dst->w = -q->w;
126 }
127 
128 static inline float quat_len(const struct quat *q)
129 {
130  float dot_val = quat_dot(q, q);
131  return (dot_val > 0.0f) ? sqrtf(dot_val) : 0.0f;
132 }
133 
134 static inline float quat_dist(const struct quat *q1, const struct quat *q2)
135 {
136  struct quat temp;
137  float dot_val;
138 
139  quat_sub(&temp, q1, q2);
140  dot_val = quat_dot(&temp, &temp);
141  return (dot_val > 0.0f) ? sqrtf(dot_val) : 0.0f;
142 }
143 
144 static inline void quat_norm(struct quat *dst, const struct quat *q)
145 {
146  float dot_val = quat_dot(q, q);
147  dst->m = (dot_val > 0.0f)
148  ? _mm_mul_ps(q->m, _mm_set1_ps(1.0f / sqrtf(dot_val)))
149  : _mm_setzero_ps();
150 }
151 
152 static inline bool quat_close(const struct quat *q1, const struct quat *q2,
153  float epsilon)
154 {
155  struct quat test;
156  quat_sub(&test, q1, q2);
157  return test.x < epsilon && test.y < epsilon && test.z < epsilon &&
158  test.w < epsilon;
159 }
160 
161 EXPORT void quat_from_axisang(struct quat *dst, const struct axisang *aa);
162 EXPORT void quat_from_matrix3(struct quat *dst, const struct matrix3 *m);
163 EXPORT void quat_from_matrix4(struct quat *dst, const struct matrix4 *m);
164 
165 EXPORT void quat_get_dir(struct vec3 *dst, const struct quat *q);
166 EXPORT void quat_set_look_dir(struct quat *dst, const struct vec3 *dir);
167 
168 EXPORT void quat_log(struct quat *dst, const struct quat *q);
169 EXPORT void quat_exp(struct quat *dst, const struct quat *q);
170 
171 EXPORT void quat_interpolate(struct quat *dst, const struct quat *q1,
172  const struct quat *q2, float t);
173 EXPORT void quat_get_tangent(struct quat *dst, const struct quat *prev,
174  const struct quat *q, const struct quat *next);
175 EXPORT void quat_interpolate_cubic(struct quat *dst, const struct quat *q1,
176  const struct quat *q2, const struct quat *m1,
177  const struct quat *m2, float t);
178 
179 #ifdef __cplusplus
180 }
181 #endif
float ptr[4]
Definition: quat.h:47
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:34
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:37
float y
Definition: quat.h:45
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:45
float x
Definition: quat.h:45
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:42
float z
Definition: quat.h:45
__m128 m
Definition: quat.h:48
EXPORT void quat_from_axisang(struct quat *dst, const struct axisang *aa)