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 {
44  float x, y, z, w;
45  };
46  float ptr[4];
47  __m128 m;
48  };
49 };
50 
51 static inline void quat_identity(struct quat *q)
52 {
53  q->m = _mm_setzero_ps();
54  q->w = 1.0f;
55 }
56 
57 static inline void quat_set(struct quat *dst, float x, float y, float z,
58  float w)
59 {
60  dst->m = _mm_set_ps(x, y, z, w);
61 }
62 
63 static inline void quat_copy(struct quat *dst, const struct quat *q)
64 {
65  dst->m = q->m;
66 }
67 
68 static inline void quat_add(struct quat *dst, const struct quat *q1,
69  const struct quat *q2)
70 {
71  dst->m = _mm_add_ps(q1->m, q2->m);
72 }
73 
74 static inline void quat_sub(struct quat *dst, const struct quat *q1,
75  const struct quat *q2)
76 {
77  dst->m = _mm_sub_ps(q1->m, q2->m);
78 }
79 
80 EXPORT void quat_mul(struct quat *dst, const struct quat *q1,
81  const struct quat *q2);
82 
83 static inline void quat_addf(struct quat *dst, const struct quat *q, float f)
84 {
85  dst->m = _mm_add_ps(q->m, _mm_set1_ps(f));
86 }
87 
88 static inline void quat_subf(struct quat *dst, const struct quat *q, 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, float f)
94 {
95  dst->m = _mm_mul_ps(q->m, _mm_set1_ps(f));
96 }
97 
98 static inline void quat_divf(struct quat *dst, const struct quat *q, float f)
99 {
100  dst->m = _mm_div_ps(q->m, _mm_set1_ps(f));
101 }
102 
103 static inline float quat_dot(const struct quat *q1, const struct quat *q2)
104 {
105  struct vec3 add;
106  __m128 mul = _mm_mul_ps(q1->m, q2->m);
107  add.m = _mm_add_ps(_mm_movehl_ps(mul, mul), mul);
108  add.m = _mm_add_ps(_mm_shuffle_ps(add.m, add.m, 0x55), add.m);
109  return add.x;
110 }
111 
112 static inline void quat_inv(struct quat *dst, const struct quat *q)
113 {
114  dst->x = -q->x;
115  dst->y = -q->y;
116  dst->z = -q->z;
117 }
118 
119 static inline void quat_neg(struct quat *dst, const struct quat *q)
120 {
121  dst->x = -q->x;
122  dst->y = -q->y;
123  dst->z = -q->z;
124  dst->w = -q->w;
125 }
126 
127 static inline float quat_len(const struct quat *q)
128 {
129  float dot_val = quat_dot(q, q);
130  return (dot_val > 0.0f) ? sqrtf(dot_val) : 0.0f;
131 }
132 
133 static inline float quat_dist(const struct quat *q1, const struct quat *q2)
134 {
135  struct quat temp;
136  float dot_val;
137 
138  quat_sub(&temp, q1, q2);
139  dot_val = quat_dot(&temp, &temp);
140  return (dot_val > 0.0f) ? sqrtf(dot_val) : 0.0f;
141 }
142 
143 static inline void quat_norm(struct quat *dst, const struct quat *q)
144 {
145  float dot_val = quat_dot(q, q);
146  dst->m = (dot_val > 0.0f)
147  ? _mm_mul_ps(q->m, _mm_set1_ps(1.0f / sqrtf(dot_val)))
148  : _mm_setzero_ps();
149 }
150 
151 static inline bool quat_close(const struct quat *q1, const struct quat *q2,
152  float epsilon)
153 {
154  struct quat test;
155  quat_sub(&test, q1, q2);
156  return test.x < epsilon && test.y < epsilon && test.z < epsilon &&
157  test.w < epsilon;
158 }
159 
160 EXPORT void quat_from_axisang(struct quat *dst, const struct axisang *aa);
161 EXPORT void quat_from_matrix3(struct quat *dst, const struct matrix3 *m);
162 EXPORT void quat_from_matrix4(struct quat *dst, const struct matrix4 *m);
163 
164 EXPORT void quat_get_dir(struct vec3 *dst, const struct quat *q);
165 EXPORT void quat_set_look_dir(struct quat *dst, const struct vec3 *dir);
166 
167 EXPORT void quat_log(struct quat *dst, const struct quat *q);
168 EXPORT void quat_exp(struct quat *dst, const struct quat *q);
169 
170 EXPORT void quat_interpolate(struct quat *dst, const struct quat *q1,
171  const struct quat *q2, float t);
172 EXPORT void quat_get_tangent(struct quat *dst, const struct quat *prev,
173  const struct quat *q, const struct quat *next);
174 EXPORT void quat_interpolate_cubic(struct quat *dst, const struct quat *q1,
175  const struct quat *q2, const struct quat *m1,
176  const struct quat *m2, float t);
177 
178 #ifdef __cplusplus
179 }
180 #endif
float ptr[4]
Definition: quat.h:46
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:44
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:44
float x
Definition: quat.h:44
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:44
__m128 m
Definition: quat.h:47
EXPORT void quat_from_axisang(struct quat *dst, const struct axisang *aa)