BRL-CAD
bn_qmath.c
Go to the documentation of this file.
1 /* B N _ Q M A T H . C
2  * BRL-CAD
3  *
4  * Copyright (c) 2014 United States Government as represented by
5  * the U.S. Army Research Laboratory.
6  *
7  * This library is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public License
9  * version 2.1 as published by the Free Software Foundation.
10  *
11  * This library is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with this file; see the file named COPYING for more
18  * information.
19  */
20 
21 #include "common.h"
22 
23 #include <stdio.h>
24 
25 #include "vmath.h"
26 #include "bn/tol.h"
27 #include "bn/qmath.h"
28 #include "bu/log.h"
29 
30 int
31 test_quat_mat2quat(int argc, const char *argv[])
32 {
33  mat_t mat;
34  quat_t expected_quat, actual_quat;
35 
36  if (argc != 2) {
37  bu_exit(1, "ERROR: input format is: mat expected_quat\n");
38  }
39  bu_scan_fastf_t(NULL, argv[0], ",", 16, &(mat[0]), &(mat[1]), &(mat[2]), &(mat[3]), &(mat[4]), &(mat[5]), &(mat[6]), &(mat[7]), &(mat[8]), &(mat[9]), &(mat[10]), &(mat[11]), &(mat[12]), &(mat[13]), &(mat[14]), &(mat[15]));
40  bu_scan_fastf_t(NULL, argv[1], "," , 4, &(expected_quat[X]), &(expected_quat[Y]), &(expected_quat[Z]), &(expected_quat[W]));
41 
42  quat_mat2quat(actual_quat, mat);
43 
44  return !HNEAR_EQUAL(expected_quat, actual_quat, BN_TOL_DIST);
45 }
46 
47 int
48 test_quat_quat2mat(int argc, const char *argv[])
49 {
50  quat_t quat;
51  mat_t expected_mat, actual_mat;
52  int i;
53 
54  if (argc != 2) {
55  bu_exit(1, "ERROR: input format is: quat expected_mat\n");
56  }
57  bu_scan_fastf_t(NULL, argv[0], "," , 4, &(quat[X]), &(quat[Y]), &quat[Z], &(quat[W]));
58  bu_scan_fastf_t(NULL, argv[1], ",", 16, &(expected_mat[0]), &(expected_mat[1]), &(expected_mat[2]), &(expected_mat[3]), &(expected_mat[4]), &(expected_mat[5]), &(expected_mat[6]), &(expected_mat[7]), &(expected_mat[8]), &(expected_mat[9]), &(expected_mat[10]), &(expected_mat[11]), &(expected_mat[12]), &(expected_mat[13]), &(expected_mat[14]), &(expected_mat[15]));
59 
60  quat_quat2mat(actual_mat, quat);
61 
62  for (i = 0; i < 16; i++) {
63  if (!NEAR_EQUAL(expected_mat[i], actual_mat[i], BN_TOL_DIST)) { return 1; }
64  }
65  return 0;
66 }
67 
68 int
69 test_quat_distance(int argc, const char *argv[])
70 {
71  quat_t q1, q2;
72  double expected_dist, actual_dist;
73 
74  if (argc != 3) {
75  bu_exit(1, "ERROR: input format is q1 q2 expected_dist\n");
76  }
77  bu_scan_fastf_t(NULL, argv[0], ",", 4, &(q1[X]), &(q1[Y]), &(q1[Z]), &(q1[W]));
78  bu_scan_fastf_t(NULL, argv[1], ",", 4, &(q2[X]), &(q2[Y]), &(q2[Z]), &(q2[W]));
79  bu_sscanf(argv[2], "%lf", &expected_dist);
80 
81  actual_dist = quat_distance(q1, q2);
82  return !(NEAR_EQUAL(actual_dist, expected_dist, BN_TOL_DIST));
83 }
84 
85 int
86 test_quat_double(int argc, const char *argv[])
87 {
88  quat_t q1, q2;
89  quat_t expected_quat, actual_quat;
90 
91  if (argc != 3) {
92  bu_exit(1, "ERROR: input format is q1 q2 expected_quat\n");
93  }
94  bu_scan_fastf_t(NULL, argv[0], ",", 4, &(q1[X]), &(q1[Y]), &(q1[Z]), &(q1[W]));
95  bu_scan_fastf_t(NULL, argv[1], ",", 4, &(q2[X]), &(q2[Y]), &(q2[Z]), &(q2[W]));
96  bu_scan_fastf_t(NULL, argv[2], ",", 4, &(expected_quat[X]), &(expected_quat[Y]), &(expected_quat[Z]), &(expected_quat[W]));
97 
98  quat_double(actual_quat, q1, q2);
99 
100  return !HNEAR_EQUAL(expected_quat, actual_quat, BN_TOL_DIST);
101 }
102 
103 int test_quat_bisect(int argc, const char *argv[])
104 {
105  quat_t q1, q2;
106  quat_t expected_quat, actual_quat;
107 
108  if (argc != 3) {
109  bu_exit(1, "ERROR: input format is q1 q2 expected_quat\n");
110  }
111  bu_scan_fastf_t(NULL, argv[0], ",", 4, &(q1[X]), &(q1[Y]), &(q1[Z]), &(q1[W]));
112  bu_scan_fastf_t(NULL, argv[1], ",", 4, &(q2[X]), &(q2[Y]), &(q2[Z]), &(q2[W]));
113  bu_scan_fastf_t(NULL, argv[2], ",", 4, &(expected_quat[X]), &(expected_quat[Y]), &(expected_quat[Z]), &(expected_quat[W]));
114 
115  quat_bisect(actual_quat, q1, q2);
116 
117  return !HNEAR_EQUAL(expected_quat, actual_quat, BN_TOL_DIST);
118 }
119 
120 int test_quat_slerp(int argc, const char *argv[])
121 {
122  quat_t q1, q2;
123  double f;
124  quat_t expected_quat, actual_quat;
125 
126  if (argc != 4) {
127  bu_exit(1, "ERROR: input format is q1 q2 f expected_quat\n");
128  }
129  bu_scan_fastf_t(NULL, argv[0], ",", 4, &(q1[X]), &(q1[Y]), &(q1[Z]), &(q1[W]));
130  bu_scan_fastf_t(NULL, argv[1], ",", 4, &(q2[X]), &(q2[Y]), &(q2[Z]), &(q2[W]));
131  bu_sscanf(argv[2], "%lf", &f);
132  bu_scan_fastf_t(NULL, argv[3], ",", 4, &(expected_quat[X]), &(expected_quat[Y]), &(expected_quat[Z]), &(expected_quat[W]));
133 
134  quat_slerp(actual_quat, q1, q2, f);
135 
136  return !HNEAR_EQUAL(expected_quat, actual_quat, BN_TOL_DIST);
137 }
138 
139 int test_quat_sberp(int argc, const char *argv[])
140 {
141  quat_t q1, qa, qb, q2;
142  double f;
143  quat_t expected_quat, actual_quat;
144 
145  if (argc != 6) {
146  bu_exit(1, "ERROR: input format is q1 qa qb q2 f expected_quat\n");
147  }
148  bu_scan_fastf_t(NULL, argv[0], ",", 4, &(q1[X]), &(q1[Y]), &(q1[Z]), &(q1[W]));
149  bu_scan_fastf_t(NULL, argv[1], ",", 4, &(qa[X]), &(qa[Y]), &(qa[Z]), &(qa[W]));
150  bu_scan_fastf_t(NULL, argv[2], ",", 4, &(qb[X]), &(qb[Y]), &(qb[Z]), &(qb[W]));
151  bu_scan_fastf_t(NULL, argv[3], ",", 4, &(q2[X]), &(q2[Y]), &(q2[Z]), &(q2[W]));
152  bu_sscanf(argv[4], "%lf", &f);
153  bu_scan_fastf_t(NULL, argv[5], ",", 4, &(expected_quat[X]), &(expected_quat[Y]), &(expected_quat[Z]), &(expected_quat[W]));
154 
155  quat_sberp(actual_quat, q1, qa, qb, q2, f);
156 
157  return !HNEAR_EQUAL(expected_quat, actual_quat, BN_TOL_DIST);
158 }
159 
160 int test_quat_make_nearest(int argc, const char *argv[])
161 {
162  quat_t q1, q2, expected_result;
163 
164  if (argc != 3) {
165  bu_exit(1, "ERROR: input format is q1 q2 expected_result\n");
166  }
167  bu_scan_fastf_t(NULL, argv[0], ",", 4, &(q1[X]), &(q1[Y]), &(q1[Z]), &(q1[W]));
168  bu_scan_fastf_t(NULL, argv[0], ",", 4, &(q2[X]), &(q2[Y]), &(q2[Z]), &(q2[W]));
169  bu_scan_fastf_t(NULL, argv[0], ",", 4, &(expected_result[X]), &(expected_result[Y]), &(expected_result[Z]), &(expected_result[W]));
170 
171  quat_make_nearest(q1, q2);
172 
173  return !HNEAR_EQUAL(expected_result, q1, BN_TOL_DIST);
174 }
175 
176 int test_quat_exp(int argc, const char *argv[])
177 {
178  quat_t in;
179  quat_t expected_out, actual_out;
180 
181  if (argc != 2) {
182  bu_exit(1, "ERROR: input format is in expected_out\n");
183  }
184  bu_scan_fastf_t(NULL, argv[0], ",", 4, &(in[X]), &(in[Y]), &(in[Z]), &(in[W]));
185  bu_scan_fastf_t(NULL, argv[1], ",", 4, &(expected_out[X]), &(expected_out[Y]), &(expected_out[Z]), &(expected_out[W]));
186 
187  quat_exp(actual_out, in);
188 
189  return !HNEAR_EQUAL(expected_out, actual_out, BN_TOL_DIST);
190 }
191 
192 int test_quat_log(int argc, const char *argv[])
193 {
194  quat_t in;
195  quat_t expected_out, actual_out;
196 
197  if (argc != 2) {
198  bu_exit(1, "ERROR: input format is in expected_out\n");
199  }
200  bu_scan_fastf_t(NULL, argv[0], ",", 4, &(in[X]), &(in[Y]), &(in[Z]), &(in[W]));
201  bu_scan_fastf_t(NULL, argv[1], ",", 4, &(expected_out[X]), &(expected_out[Y]), &(expected_out[Z]), &(expected_out[W]));
202 
203  quat_log(actual_out, in);
204 
205  return !HNEAR_EQUAL(expected_out, actual_out, BN_TOL_DIST);
206 }
207 
208 int
209 main(int argc, const char *argv[])
210 {
211  int function_num = 0;
212 
213  if (argc < 3) {
214  bu_exit(1, "ERROR: input format is function_num function_test_args [%s]\n", argv[0]);
215  }
216 
217  sscanf(argv[1], "%d", &function_num);
218 
219  switch (function_num) {
220  case 1:
221  return test_quat_mat2quat(argc - 2, argv + 2);
222  case 2:
223  return test_quat_quat2mat(argc - 2, argv + 2);
224  case 3:
225  return test_quat_distance(argc - 2, argv + 2);
226  case 4:
227  return test_quat_double(argc - 2, argv + 2);
228  case 5:
229  return test_quat_bisect(argc - 2, argv + 2);
230  case 6:
231  return test_quat_slerp(argc - 2, argv + 2);
232  case 7:
233  return test_quat_sberp(argc - 2, argv + 2);
234  case 8:
235  return test_quat_make_nearest(argc - 2, argv + 2);
236  case 9:
237  return test_quat_exp(argc - 2, argv + 2);
238  case 10:
239  return test_quat_log(argc - 2, argv + 2);
240  default:
241  bu_exit(1, "ERROR: Unrecognized function_num\n");
242  }
243 
244  return 1;
245 }
246 
247 
248 /** @} */
249 /*
250  * Local Variables:
251  * mode: C
252  * tab-width: 8
253  * indent-tabs-mode: t
254  * c-file-style: "stroustrup"
255  * End:
256  * ex: shiftwidth=4 tabstop=8
257  */
int test_quat_exp(int argc, const char *argv[])
Definition: bn_qmath.c:176
int test_quat_log(int argc, const char *argv[])
Definition: bn_qmath.c:192
int test_quat_quat2mat(int argc, const char *argv[])
Definition: bn_qmath.c:48
void quat_sberp(quat_t qout, const quat_t q1, const quat_t qa, const quat_t qb, const quat_t q2, double f)
Spherical Bezier Interpolate between four quaternions by amount f. These are intended to be used as s...
Header file for the BRL-CAD common definitions.
void quat_bisect(quat_t qout, const quat_t q1, const quat_t q2)
Gives the bisector of quaternions q1 and q2. (Could be done with quat_slerp and factor 0...
int test_quat_slerp(int argc, const char *argv[])
Definition: bn_qmath.c:120
int test_quat_sberp(int argc, const char *argv[])
Definition: bn_qmath.c:139
Definition: color.c:49
int test_quat_mat2quat(int argc, const char *argv[])
Definition: bn_qmath.c:31
void bu_exit(int status, const char *fmt,...) _BU_ATTR_NORETURN _BU_ATTR_PRINTF23
Definition: bomb.c:195
int bu_sscanf(const char *src, const char *fmt,...) _BU_ATTR_SCANF23
Definition: sscanf.c:676
#define BN_TOL_DIST
Definition: tol.h:109
int bu_scan_fastf_t(int *c, const char *src, const char *delim, int n,...)
Definition: scan.c:32
int test_quat_bisect(int argc, const char *argv[])
Definition: bn_qmath.c:103
int test_quat_distance(int argc, const char *argv[])
Definition: bn_qmath.c:69
void quat_double(quat_t qout, const quat_t q1, const quat_t q2)
Gives the quaternion point representing twice the rotation from q1 to q2. Needed for patching Bezier ...
int test_quat_double(int argc, const char *argv[])
Definition: bn_qmath.c:86
int test_quat_make_nearest(int argc, const char *argv[])
Definition: bn_qmath.c:160
int main(int argc, const char *argv[])
Definition: bn_qmath.c:209
void quat_slerp(quat_t qout, const quat_t q1, const quat_t q2, double f)
Do Spherical Linear Interpolation between two unit quaternions by the given factor.
void quat_quat2mat(mat_t mat, const quat_t quat)
Convert Quaternion to Matrix.
Definition: color.c:51
double quat_distance(const quat_t q1, const quat_t q2)
Gives the euclidean distance between two quaternions.
void quat_exp(quat_t out, const quat_t in)
Exponentiate a quaternion, assuming that the scalar part is 0. Code by Ken Shoemake.
void quat_mat2quat(quat_t quat, const mat_t mat)
Quaternion math routines.
void quat_log(quat_t out, const quat_t in)
Take the natural logarithm of a unit quaternion. Code by Ken Shoemake.
void quat_make_nearest(quat_t q1, const quat_t q2)
Set the quaternion q1 to the quaternion which yields the smallest rotation from q2 (of the two versio...
Definition: color.c:50