test_sanim_node_pivot.c (21946B)
1 /* Copyright (C) 2018-2026 |Méso|Star> (contact@meso-star.com) 2 * Copyright (C) 2016-2018 CNRS 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 3 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 #include "sanim.h" 18 #include "test_sanim_utils.h" 19 20 #include <rsys/double33.h> 21 22 int 23 main(int argc, char** argv) 24 { 25 struct mem_allocator allocator; 26 struct my_type *t1, *t2, *t3, *target; 27 struct sanim_pivot pivot = SANIM_PIVOT_NULL; 28 struct sanim_tracking tracking = SANIM_TRACKING_NULL; 29 double transform[12]; 30 double transl[3], rot[3], in_dir[3], n[3], tmp[3]; 31 const double y_ref_normal[3] = { 0, 1, 0 }; 32 (void) argc, (void) argv; 33 34 mem_init_proxy_allocator(&allocator, &mem_default_allocator); 35 36 /* 37 * 1 axis pivots 38 * (in/out dirs can be off the YZ plane) 39 */ 40 41 /* 1 axis tracking sun */ 42 43 tracking.policy = TRACKING_SUN; 44 pivot.type = PIVOT_SINGLE_AXIS; 45 d3(pivot.data.pivot1.ref_normal, 0, 0, 1); 46 47 /* ref_normal not in the YZ plane */ 48 d3(pivot.data.pivot1.ref_normal, 1, 0, 1); 49 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t1) == RES_BAD_ARG); 50 d3(pivot.data.pivot1.ref_normal, 0, 0, 1); 51 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t1) == RES_OK); 52 CHK(my_type_ref_put(t1) == RES_OK); 53 CHK(my_type_pivot_create(NULL, &pivot, &tracking, &t1) == RES_OK); 54 CHK(my_type_ref_put(t1) == RES_OK); 55 56 CHK(my_type_create(&allocator, &t1) == RES_OK); 57 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 58 CHK(my_type_create(&allocator, &t3) == RES_OK); 59 60 CHK(my_type_add_child(t1, t2) == RES_OK); 61 CHK(my_type_add_child(t2, t3) == RES_OK); 62 63 d3_splat(transl, +1); 64 d3(rot, 0, 0, PI/2); 65 CHK(my_type_set_translation(t1, transl) == RES_OK); 66 CHK(my_type_set_rotations(t1, rot) == RES_OK); 67 CHK(my_type_set_translation(t2, transl) == RES_OK); 68 CHK(my_type_set_translation(t3, transl) == RES_OK); 69 70 d3(in_dir, 0, 0.99, -0.1); 71 /* rotation axis is Y after positioning: cannot accomodate in_dir */ 72 CHK(my_type_solve_pivot(t2, in_dir) == RES_BAD_ARG); 73 74 d3(in_dir, 1, 0.2, -1); 75 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 76 CHK(my_type_get_transform(t3, transform) == RES_OK); 77 d33_muld3(n, transform, pivot.data.pivot1.ref_normal); 78 CHK(d3_eq_eps(n, d3(tmp, -sqrt(2) / 2, 0, +sqrt(2) / 2), 1e-7) == 1); 79 CHK(d3_eq_eps(transform + 9, d3(tmp, -sqrt(2), 3, 2), 1e-7) == 1); 80 81 CHK(my_type_ref_put(t1) == RES_OK); 82 CHK(my_type_ref_put(t2) == RES_OK); 83 CHK(my_type_ref_put(t3) == RES_OK); 84 85 /* 1 axis tracking with a fixed output dir */ 86 87 tracking.policy = TRACKING_OUT_DIR; 88 pivot.type = PIVOT_SINGLE_AXIS; 89 d3(pivot.data.pivot1.ref_normal, 0, 0, 1); 90 d3(tracking.data.out_dir.u, 0, 1, 0); 91 92 CHK(my_type_create(&allocator, &t1) == RES_OK); 93 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 94 CHK(my_type_create(&allocator, &t3) == RES_OK); 95 96 CHK(my_type_add_child(t1, t2) == RES_OK); 97 CHK(my_type_add_child(t2, t3) == RES_OK); 98 99 CHK(my_type_set_translation(t1, transl) == RES_OK); 100 CHK(my_type_set_rotations(t1, rot) == RES_OK); 101 CHK(my_type_set_translation(t2, transl) == RES_OK); 102 CHK(my_type_set_translation(t3, transl) == RES_OK); 103 104 d3(in_dir, 0, -1, -0.1); 105 /* rotation axis is Y after positioning: cannot accomodate <in_dir,out_dir> */ 106 CHK(my_type_solve_pivot(t2, in_dir) == RES_BAD_ARG); 107 108 CHK(my_type_ref_put(t1) == RES_OK); 109 CHK(my_type_ref_put(t2) == RES_OK); 110 CHK(my_type_ref_put(t3) == RES_OK); 111 112 tracking.policy = TRACKING_OUT_DIR; 113 pivot.type = PIVOT_SINGLE_AXIS; 114 d3(pivot.data.pivot1.ref_normal, 0, 0, 1); 115 d3(tracking.data.out_dir.u, 1, 0, 1); 116 117 CHK(my_type_create(&allocator, &t1) == RES_OK); 118 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 119 CHK(my_type_create(&allocator, &t3) == RES_OK); 120 121 CHK(my_type_add_child(t1, t2) == RES_OK); 122 CHK(my_type_add_child(t2, t3) == RES_OK); 123 124 CHK(my_type_set_translation(t1, transl) == RES_OK); 125 CHK(my_type_set_rotations(t1, rot) == RES_OK); 126 CHK(my_type_set_translation(t2, transl) == RES_OK); 127 CHK(my_type_set_translation(t3, transl) == RES_OK); 128 129 d3(in_dir, 1, -0.3, -1); 130 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 131 CHK(my_type_get_transform(t3, transform) == RES_OK); 132 d33_muld3(n, transform, pivot.data.pivot1.ref_normal); 133 CHK(d3_eq_eps(pivot.data.pivot1.ref_normal, n, 1e-7) == 1); 134 CHK(d3_eq_eps(transform + 9, d3(n, -1, 3, 3), 1e-7) == 1); 135 136 CHK(my_type_ref_put(t1) == RES_OK); 137 CHK(my_type_ref_put(t2) == RES_OK); 138 CHK(my_type_ref_put(t3) == RES_OK); 139 140 /* 1 axis tracking a target point */ 141 142 tracking.policy = TRACKING_POINT; 143 pivot.type = PIVOT_SINGLE_AXIS; 144 d3(pivot.data.pivot1.ref_normal, 0, 0, 1); 145 d3(pivot.data.pivot1.ref_point, 0, 0, 0); 146 d3(tracking.data.point.target, 0, 0, 30); 147 tracking.data.point.target_is_local = 1; 148 149 CHK(my_type_create(&allocator, &t1) == RES_OK); 150 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 151 CHK(my_type_create(&allocator, &t3) == RES_OK); 152 153 CHK(my_type_add_child(t1, t2) == RES_OK); 154 CHK(my_type_add_child(t2, t3) == RES_OK); 155 156 CHK(my_type_set_translation(t1, transl) == RES_OK); 157 CHK(my_type_set_rotations(t1, rot) == RES_OK); 158 CHK(my_type_set_translation(t2, transl) == RES_OK); 159 CHK(my_type_set_translation(t3, transl) == RES_OK); 160 161 d3(in_dir, 1, 0.1, 0); 162 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 163 CHK(my_type_get_transform(t3, transform) == RES_OK); 164 d33_muld3(n, transform, pivot.data.pivot1.ref_normal); 165 CHK(d3_eq_eps(n, d3(tmp, -sqrt(2) / 2, 0, +sqrt(2) / 2), 1e-7) == 1); 166 167 CHK(my_type_ref_put(t1) == RES_OK); 168 CHK(my_type_ref_put(t2) == RES_OK); 169 CHK(my_type_ref_put(t3) == RES_OK); 170 171 tracking.policy = TRACKING_POINT; 172 pivot.type = PIVOT_SINGLE_AXIS; 173 d3(pivot.data.pivot1.ref_normal, 0, 0, 1); 174 d3(pivot.data.pivot1.ref_point, 0, 0, 10 * sqrt(2)); 175 d3(tracking.data.point.target, 0, 10, 30); 176 tracking.data.point.target_is_local = 1; 177 178 CHK(my_type_create(&allocator, &t1) == RES_OK); 179 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 180 CHK(my_type_create(&allocator, &t3) == RES_OK); 181 182 CHK(my_type_add_child(t1, t2) == RES_OK); 183 CHK(my_type_add_child(t2, t3) == RES_OK); 184 185 CHK(my_type_set_translation(t1, transl) == RES_OK); 186 CHK(my_type_set_rotations(t1, rot) == RES_OK); 187 CHK(my_type_set_translation(t2, transl) == RES_OK); 188 CHK(my_type_set_translation(t3, transl) == RES_OK); 189 190 d3(in_dir, 1, 0.1, 0); 191 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 192 CHK(my_type_get_transform(t3, transform) == RES_OK); 193 d33_muld3(n, transform, pivot.data.pivot1.ref_normal); 194 CHK(d3_eq_eps(n, d3(tmp, -sqrt(2) / 2, 0, +sqrt(2) / 2), 1e-7) == 1); 195 196 CHK(my_type_ref_put(t1) == RES_OK); 197 CHK(my_type_ref_put(t2) == RES_OK); 198 CHK(my_type_ref_put(t3) == RES_OK); 199 200 /* same 1 axis tracking with a non-local target point */ 201 202 tracking.policy = TRACKING_POINT; 203 pivot.type = PIVOT_SINGLE_AXIS; 204 d3(pivot.data.pivot1.ref_normal, 0, 0, 1); 205 d3(pivot.data.pivot1.ref_point, 0, 0, 10 * sqrt(2)); 206 d3(tracking.data.point.target, -10, 2, 32); 207 tracking.data.point.target_is_local = 0; 208 209 CHK(my_type_create(&allocator, &t1) == RES_OK); 210 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 211 CHK(my_type_create(&allocator, &t3) == RES_OK); 212 213 CHK(my_type_add_child(t1, t2) == RES_OK); 214 CHK(my_type_add_child(t2, t3) == RES_OK); 215 216 CHK(my_type_set_translation(t1, transl) == RES_OK); 217 CHK(my_type_set_rotations(t1, rot) == RES_OK); 218 CHK(my_type_set_translation(t2, transl) == RES_OK); 219 CHK(my_type_set_translation(t3, transl) == RES_OK); 220 221 d3(in_dir, 1, -0.5, 0); 222 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 223 CHK(my_type_get_transform(t3, transform) == RES_OK); 224 d33_muld3(n, transform, pivot.data.pivot1.ref_normal); 225 CHK(d3_eq_eps(n, d3(tmp, -sqrt(2) / 2, 0, +sqrt(2) / 2), 1e-7) == 1); 226 227 CHK(my_type_ref_put(t1) == RES_OK); 228 CHK(my_type_ref_put(t2) == RES_OK); 229 CHK(my_type_ref_put(t3) == RES_OK); 230 231 tracking.policy = TRACKING_POINT; 232 pivot.type = PIVOT_SINGLE_AXIS; 233 d3(pivot.data.pivot1.ref_normal, 0, 0, 1); 234 d3(pivot.data.pivot1.ref_point, 0, 5, 5); 235 d3(tracking.data.point.target, -12, 2, -10); 236 tracking.data.point.target_is_local = 0; 237 238 CHK(my_type_create(&allocator, &t1) == RES_OK); 239 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 240 CHK(my_type_create(&allocator, &t3) == RES_OK); 241 242 CHK(my_type_add_child(t1, t2) == RES_OK); 243 CHK(my_type_add_child(t2, t3) == RES_OK); 244 245 CHK(my_type_set_translation(t1, transl) == RES_OK); 246 CHK(my_type_set_rotations(t1, rot) == RES_OK); 247 CHK(my_type_set_translation(t2, transl) == RES_OK); 248 CHK(my_type_set_translation(t3, transl) == RES_OK); 249 250 d3(in_dir, 1, 0, -1); 251 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 252 CHK(my_type_get_transform(t3, transform) == RES_OK); 253 d33_muld3(n, transform, pivot.data.pivot1.ref_normal); 254 CHK(d3_eq_eps(n, d3(tmp, -1, 0, 0), 1e-7) == 1); 255 CHK(d3_eq_eps(transform + 9, d3(tmp, -1, 3, 1), 1e-7) == 1); 256 257 CHK(my_type_ref_put(t1) == RES_OK); 258 CHK(my_type_ref_put(t2) == RES_OK); 259 CHK(my_type_ref_put(t3) == RES_OK); 260 261 /* 1 axis tracking an invalid node target */ 262 263 CHK(my_type_create(&allocator, &t1) == RES_OK); 264 CHK(my_type_create(&allocator, &t3) == RES_OK); 265 d3(pivot.data.pivot1.ref_normal, 0, 0, 1); 266 d3(pivot.data.pivot1.ref_point, 0, 0, 0); 267 CHK(my_type_track_me(t3, &tracking) == RES_OK); 268 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 269 270 CHK(my_type_add_child(t1, t2) == RES_OK); 271 CHK(my_type_add_child(t2, t3) == RES_OK); 272 273 CHK(my_type_set_translation(t1, transl) == RES_OK); 274 CHK(my_type_set_rotations(t1, rot) == RES_OK); 275 CHK(my_type_set_translation(t2, transl) == RES_OK); 276 CHK(my_type_set_translation(t3, transl) == RES_OK); 277 278 d3(in_dir, 1, 0.1, 0); 279 /* target if after a pivot */ 280 CHK(my_type_solve_pivot(t2, in_dir) == RES_BAD_ARG); 281 282 CHK(my_type_ref_put(t1) == RES_OK); 283 CHK(my_type_ref_put(t2) == RES_OK); 284 CHK(my_type_ref_put(t3) == RES_OK); 285 286 /* 1 axis tracking a node target */ 287 288 CHK(my_type_create(&allocator, &target) == RES_OK); 289 d3(tmp, 0, 0, 10 * sqrt(2)); 290 CHK(my_type_set_translation(target, tmp) == RES_OK); 291 292 d3(pivot.data.pivot1.ref_normal, 0, 0, 1); 293 d3(pivot.data.pivot1.ref_point, 0, 0, 0); 294 CHK(my_type_track_me(target, &tracking) == RES_OK); 295 296 CHK(my_type_create(&allocator, &t1) == RES_OK); 297 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 298 CHK(my_type_create(&allocator, &t3) == RES_OK); 299 300 CHK(my_type_add_child(t1, t2) == RES_OK); 301 CHK(my_type_add_child(t2, t3) == RES_OK); 302 303 CHK(my_type_set_translation(t1, transl) == RES_OK); 304 CHK(my_type_set_rotations(t1, rot) == RES_OK); 305 CHK(my_type_set_translation(t2, transl) == RES_OK); 306 CHK(my_type_set_translation(t3, transl) == RES_OK); 307 308 d3(in_dir, 1, 0.1, 0); 309 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 310 CHK(my_type_get_transform(t3, transform) == RES_OK); 311 d33_muld3(n, transform, pivot.data.pivot1.ref_normal); 312 CHK(d3_eq_eps(n, d3(tmp, -sqrt(2) / 2, 0, +sqrt(2) / 2), 1e-7) == 1); 313 314 CHK(my_type_ref_put(t1) == RES_OK); 315 CHK(my_type_ref_put(t2) == RES_OK); 316 CHK(my_type_ref_put(t3) == RES_OK); 317 CHK(my_type_ref_put(target) == RES_OK); 318 319 /* 320 * 2 axis pivots 321 * (using only one axis at a time) 322 */ 323 324 /* 2 axis tracking sun */ 325 326 tracking.policy = TRACKING_SUN; 327 pivot.type = PIVOT_TWO_AXIS; 328 pivot.data.pivot2.spacing = 1; 329 d3(pivot.data.pivot2.ref_point, 0, 0, 1); 330 331 CHK(my_type_create(&allocator, &t1) == RES_OK); 332 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 333 CHK(my_type_create(&allocator, &t3) == RES_OK); 334 335 CHK(my_type_add_child(t1, t2) == RES_OK); 336 CHK(my_type_add_child(t2, t3) == RES_OK); 337 338 d3_splat(transl, +1); 339 d3(rot, 0, 0, PI / 2); 340 CHK(my_type_set_translation(t1, transl) == RES_OK); 341 CHK(my_type_set_rotations(t1, rot) == RES_OK); 342 CHK(my_type_set_translation(t2, transl) == RES_OK); 343 CHK(my_type_set_translation(t3, transl) == RES_OK); 344 345 d3(in_dir, 1, 0, -1); 346 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 347 CHK(my_type_get_transform(t3, transform) == RES_OK); 348 d33_muld3(n, transform, y_ref_normal); 349 CHK(d3_eq_eps(n, d3(tmp, -sqrt(2) / 2, 0, +sqrt(2) / 2), 1e-7) == 1); 350 CHK(d3_eq_eps(transform + 9, d3(tmp, -1, 3, 2 + sqrt(2)), 1e-7) == 1); 351 352 d3(in_dir, 0, 1, 0); 353 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 354 CHK(my_type_get_transform(t3, transform) == RES_OK); 355 d33_muld3(n, transform, y_ref_normal); 356 CHK(d3_eq_eps(n, d3(tmp, 0, -1, 0), 1e-7) == 1); 357 CHK(d3_eq_eps(transform + 9, d3(tmp, -1, 0, 3), 1e-7) == 1); 358 359 CHK(my_type_ref_put(t1) == RES_OK); 360 CHK(my_type_ref_put(t2) == RES_OK); 361 CHK(my_type_ref_put(t3) == RES_OK); 362 363 /* 2 axis tracking sun */ 364 365 tracking.policy = TRACKING_SUN; 366 pivot.type = PIVOT_TWO_AXIS; 367 pivot.data.pivot2.spacing = 1; 368 369 CHK(my_type_create(&allocator, &t1) == RES_OK); 370 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 371 CHK(my_type_create(&allocator, &t3) == RES_OK); 372 373 CHK(my_type_add_child(t1, t2) == RES_OK); 374 CHK(my_type_add_child(t2, t3) == RES_OK); 375 376 d3_splat(transl, +1); 377 d3(rot, 0, 0, PI / 2); 378 CHK(my_type_set_translation(t1, transl) == RES_OK); 379 CHK(my_type_set_rotations(t1, rot) == RES_OK); 380 CHK(my_type_set_translation(t2, transl) == RES_OK); 381 CHK(my_type_set_translation(t3, transl) == RES_OK); 382 383 d3(in_dir, 1, 0, -1); 384 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 385 CHK(my_type_get_transform(t3, transform) == RES_OK); 386 d33_muld3(n, transform, y_ref_normal); 387 CHK(d3_eq_eps(n, d3(tmp, -sqrt(2) / 2, 0, +sqrt(2) / 2), 1e-7) == 1); 388 CHK(d3_eq_eps(transform + 9, d3(tmp, -1, 3, 2 + sqrt(2)), 1e-7) == 1); 389 390 d3(in_dir, 0, 1, 0); 391 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 392 CHK(my_type_get_transform(t3, transform) == RES_OK); 393 d33_muld3(n, transform, y_ref_normal); 394 CHK(d3_eq_eps(n, d3(tmp, 0, -1, 0), 1e-7) == 1); 395 CHK(d3_eq_eps(transform + 9, d3(tmp, -1, 0, 3), 1e-7) == 1); 396 397 CHK(my_type_ref_put(t1) == RES_OK); 398 CHK(my_type_ref_put(t2) == RES_OK); 399 CHK(my_type_ref_put(t3) == RES_OK); 400 401 /* 2 axis tracking with a fixed output dir */ 402 403 tracking.policy = TRACKING_OUT_DIR; 404 pivot.type = PIVOT_TWO_AXIS; 405 pivot.data.pivot2.spacing = 1; 406 d3(tracking.data.out_dir.u, 0, 1, 0); 407 408 CHK(my_type_create(&allocator, &t1) == RES_OK); 409 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 410 CHK(my_type_create(&allocator, &t3) == RES_OK); 411 412 CHK(my_type_add_child(t1, t2) == RES_OK); 413 CHK(my_type_add_child(t2, t3) == RES_OK); 414 415 CHK(my_type_set_translation(t1, transl) == RES_OK); 416 CHK(my_type_set_rotations(t1, rot) == RES_OK); 417 CHK(my_type_set_translation(t2, transl) == RES_OK); 418 CHK(my_type_set_translation(t3, transl) == RES_OK); 419 420 d3(in_dir, 0, 0, -1); 421 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 422 CHK(my_type_get_transform(t3, transform) == RES_OK); 423 d33_muld3(n, transform, y_ref_normal); 424 CHK(d3_eq_eps(n, d3(tmp, 0, sqrt(2) / 2, sqrt(2) / 2), 1e-7) == 1); 425 CHK(d3_eq_eps(transform + 9, d3(tmp, 1, 3, 2 + sqrt(2)), 1e-7) == 1); 426 427 d3(in_dir, -1, 0, 0); 428 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 429 CHK(my_type_get_transform(t3, transform) == RES_OK); 430 d33_muld3(n, transform, y_ref_normal); 431 CHK(d3_eq_eps(n, d3(tmp, sqrt(2) / 2, sqrt(2) / 2, 0), 1e-7) == 1); 432 CHK(d3_eq_eps(transform + 9, d3(tmp, 3 * sqrt(2) / 2, 2 + sqrt(2) / 2, 3), 1e-7) == 1); 433 434 CHK(my_type_ref_put(t1) == RES_OK); 435 CHK(my_type_ref_put(t2) == RES_OK); 436 CHK(my_type_ref_put(t3) == RES_OK); 437 438 /* 2 axis tracking a target point */ 439 440 tracking.policy = TRACKING_POINT; 441 pivot.type = PIVOT_TWO_AXIS; 442 pivot.data.pivot2.spacing = 0; 443 d3(pivot.data.pivot2.ref_point, 0, 0, 0); 444 d3(tracking.data.point.target, 30, 0, 0); 445 tracking.data.point.target_is_local = 1; 446 447 CHK(my_type_create(&allocator, &t1) == RES_OK); 448 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 449 CHK(my_type_create(&allocator, &t3) == RES_OK); 450 451 CHK(my_type_add_child(t1, t2) == RES_OK); 452 CHK(my_type_add_child(t2, t3) == RES_OK); 453 454 CHK(my_type_set_translation(t1, transl) == RES_OK); 455 CHK(my_type_set_rotations(t1, rot) == RES_OK); 456 CHK(my_type_set_translation(t2, transl) == RES_OK); 457 CHK(my_type_set_translation(t3, transl) == RES_OK); 458 459 d3(in_dir, -1, 0, 0); 460 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 461 CHK(my_type_get_transform(t3, transform) == RES_OK); 462 d33_muld3(n, transform, y_ref_normal); 463 CHK(d3_eq_eps(n, d3(tmp, sqrt(2) / 2, sqrt(2) / 2, 0), 1e-7) == 1); 464 CHK(d3_eq_eps(transform + 9, d3(tmp, sqrt(2), 2, 3), 1e-7) == 1); 465 466 CHK(my_type_ref_put(t1) == RES_OK); 467 CHK(my_type_ref_put(t2) == RES_OK); 468 CHK(my_type_ref_put(t3) == RES_OK); 469 470 tracking.policy = TRACKING_POINT; 471 pivot.type = PIVOT_TWO_AXIS; 472 pivot.data.pivot2.spacing = sqrt(2); 473 d3(pivot.data.pivot2.ref_point, 0, 10 * sqrt(2), 0); 474 d3(tracking.data.point.target, 30, -11, 0); 475 tracking.data.point.target_is_local = 1; 476 477 CHK(my_type_create(&allocator, &t1) == RES_OK); 478 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 479 CHK(my_type_create(&allocator, &t3) == RES_OK); 480 481 CHK(my_type_add_child(t1, t2) == RES_OK); 482 CHK(my_type_add_child(t2, t3) == RES_OK); 483 484 CHK(my_type_set_translation(t1, transl) == RES_OK); 485 CHK(my_type_set_rotations(t1, rot) == RES_OK); 486 CHK(my_type_set_translation(t2, transl) == RES_OK); 487 CHK(my_type_set_translation(t3, transl) == RES_OK); 488 489 d3(in_dir, -1, 0, 0); 490 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 491 CHK(my_type_get_transform(t3, transform) == RES_OK); 492 d33_muld3(n, transform, y_ref_normal); 493 CHK(d3_eq_eps(n, d3(tmp, sqrt(2) / 2, sqrt(2) / 2, 0), 1e-7) == 1); 494 CHK(d3_eq_eps(transform + 9, d3(tmp, 1 + sqrt(2), 3, 3), 1e-7) == 1); 495 496 CHK(my_type_ref_put(t1) == RES_OK); 497 CHK(my_type_ref_put(t2) == RES_OK); 498 CHK(my_type_ref_put(t3) == RES_OK); 499 500 tracking.policy = TRACKING_POINT; 501 pivot.type = PIVOT_TWO_AXIS; 502 pivot.data.pivot2.spacing = 1; 503 d3(pivot.data.pivot2.ref_point, 0, 10 * sqrt(2), 0); 504 d3(tracking.data.point.target, 0, 30, 10); 505 tracking.data.point.target_is_local = 1; 506 507 CHK(my_type_create(&allocator, &t1) == RES_OK); 508 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 509 CHK(my_type_create(&allocator, &t3) == RES_OK); 510 511 CHK(my_type_add_child(t1, t2) == RES_OK); 512 CHK(my_type_add_child(t2, t3) == RES_OK); 513 514 CHK(my_type_set_translation(t1, transl) == RES_OK); 515 CHK(my_type_set_rotations(t1, rot) == RES_OK); 516 CHK(my_type_set_translation(t2, transl) == RES_OK); 517 CHK(my_type_set_translation(t3, transl) == RES_OK); 518 519 d3(in_dir, 0, 0, -1); 520 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 521 CHK(my_type_get_transform(t3, transform) == RES_OK); 522 d33_muld3(n, transform, y_ref_normal); 523 CHK(d3_eq_eps(n, d3(tmp, -sqrt(2) / 2, 0, +sqrt(2) / 2), 1e-7) == 1); 524 CHK(d3_eq_eps(transform + 9, d3(tmp, -1, 3, 2 + sqrt(2)), 1e-7) == 1); 525 526 CHK(my_type_ref_put(t1) == RES_OK); 527 CHK(my_type_ref_put(t2) == RES_OK); 528 CHK(my_type_ref_put(t3) == RES_OK); 529 530 tracking.policy = TRACKING_POINT; 531 pivot.type = PIVOT_TWO_AXIS; 532 pivot.data.pivot2.spacing = 1; 533 d3(pivot.data.pivot2.ref_point, 0, 10 * sqrt(2), 0); 534 d3(tracking.data.point.target, 0, 30, 12); 535 tracking.data.point.target_is_local = 0; 536 537 CHK(my_type_create(&allocator, &t1) == RES_OK); 538 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 539 CHK(my_type_create(&allocator, &t3) == RES_OK); 540 541 CHK(my_type_add_child(t1, t2) == RES_OK); 542 CHK(my_type_add_child(t2, t3) == RES_OK); 543 544 CHK(my_type_set_translation(t1, transl) == RES_OK); 545 CHK(my_type_set_rotations(t1, rot) == RES_OK); 546 CHK(my_type_set_translation(t2, transl) == RES_OK); 547 CHK(my_type_set_translation(t3, transl) == RES_OK); 548 549 d3(in_dir, 0, 0, -1); 550 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 551 CHK(my_type_get_transform(t3, transform) == RES_OK); 552 d33_muld3(n, transform, y_ref_normal); 553 CHK(d3_eq_eps(n, d3(tmp, 0, sqrt(2) / 2, sqrt(2) / 2), 1e-7) == 1); 554 CHK(d3_eq_eps(transform + 9, d3(tmp, 1, 3, 2 + sqrt(2)), 1e-7) == 1); 555 556 CHK(my_type_ref_put(t1) == RES_OK); 557 CHK(my_type_ref_put(t2) == RES_OK); 558 CHK(my_type_ref_put(t3) == RES_OK); 559 560 tracking.policy = TRACKING_POINT; 561 pivot.type = PIVOT_TWO_AXIS; 562 pivot.data.pivot2.spacing = 1; 563 d3(pivot.data.pivot2.ref_point, 10, 8 * sqrt(2), 5 * sqrt(2)); 564 d3(tracking.data.point.target, 10, 12, 15); 565 tracking.data.point.target_is_local = 0; 566 567 CHK(my_type_create(&allocator, &t1) == RES_OK); 568 CHK(my_type_pivot_create(&allocator, &pivot, &tracking, &t2) == RES_OK); 569 CHK(my_type_create(&allocator, &t3) == RES_OK); 570 571 CHK(my_type_add_child(t1, t2) == RES_OK); 572 CHK(my_type_add_child(t2, t3) == RES_OK); 573 574 CHK(my_type_set_translation(t1, transl) == RES_OK); 575 CHK(my_type_set_rotations(t1, rot) == RES_OK); 576 CHK(my_type_set_translation(t2, transl) == RES_OK); 577 CHK(my_type_set_translation(t3, transl) == RES_OK); 578 579 d3(in_dir, 0, 0, -1); 580 CHK(my_type_solve_pivot(t2, in_dir) == RES_OK); 581 CHK(my_type_get_transform(t3, transform) == RES_OK); 582 d33_muld3(n, transform, y_ref_normal); 583 CHK(d3_eq_eps(n, d3(tmp, 0, sqrt(2) / 2, sqrt(2) / 2), 1e-7) == 1); 584 CHK(d3_eq_eps(transform + 9, d3(tmp, 1, 3, 2 + sqrt(2)), 1e-7) == 1); 585 586 /* release memory */ 587 CHK(my_type_ref_put(t1) == RES_OK); 588 CHK(my_type_ref_put(t2) == RES_OK); 589 CHK(my_type_ref_put(t3) == RES_OK); 590 check_memory_allocator(&allocator); 591 mem_shutdown_proxy_allocator(&allocator); 592 CHK(mem_allocated_size() == 0); 593 return 0; 594 }