sanim_node.c (33374B)
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_node_c.h" 18 #include "sanim.h" 19 20 #include <rsys/mem_allocator.h> 21 #include <rsys/ref_count.h> 22 #include <rsys/double33.h> 23 #include <rsys/double22.h> 24 25 #include <math.h> 26 27 /******************************************************************************* 28 * Helper functions 29 ******************************************************************************/ 30 static int 31 is_ancestor 32 (const struct sanim_node* node, const struct sanim_node* possible_ancestor) 33 { 34 ASSERT(node && node->data && possible_ancestor); 35 while (node) { 36 if (node == possible_ancestor) return 1; 37 node = node->data->father; 38 } 39 return 0; 40 } 41 42 static int 43 is_after_pivot(const struct sanim_node* node) 44 { 45 ASSERT(node); 46 while (node) { 47 if (node->data->pivot_data) return 1; 48 node = node->data->father; 49 } 50 return 0; 51 } 52 53 static void 54 d34_muld34(double dst[12], const double a[12], const double b[12]) 55 { 56 double tmp[3]; 57 ASSERT(dst && a && b); 58 d3_add(dst + 9, d33_muld3(tmp, a, b + 9), a + 9); 59 d33_muld33(dst, a, b); 60 } 61 62 static INLINE void 63 d34_set_identity(double dst[12]) 64 { 65 ASSERT(dst); 66 d33_set_identity(dst); 67 d3_splat(dst + 9, 0); 68 } 69 70 static double* 71 get_Xpivot_transform 72 (const double angle, 73 const double spacing, 74 double transform[12]) 75 { 76 ASSERT(transform); 77 d33_rotation_pitch(transform, angle); 78 d3(transform + 9, 0, spacing, 0); 79 return transform; 80 } 81 82 static double* 83 compose_Xpivot_transform_L 84 (const double angle, 85 const double spacing, 86 double accum[12]) 87 { 88 double pivot[12]; 89 ASSERT(accum); 90 get_Xpivot_transform(angle, spacing, pivot); 91 d34_muld34(accum, pivot, accum); 92 return accum; 93 } 94 95 static double* 96 get_Zpivot_transform(const double angle, double transform[12]) { 97 ASSERT(transform); 98 d33_rotation_roll(transform, angle); 99 d3_splat(transform + 9, 0); 100 return transform; 101 } 102 103 static double* 104 compose_Zpivot_transform_L(const double angle, double accum[12]) { 105 double pivot[12]; 106 ASSERT(accum); 107 get_Zpivot_transform(angle, pivot); 108 d34_muld34(accum, pivot, accum); 109 return accum; 110 } 111 112 static double* 113 get_ZXpivot_transform 114 (const double angleZ, 115 const double angleX, 116 const double spacing, 117 double transform[12]) 118 { 119 ASSERT(transform); 120 get_Xpivot_transform(angleX, spacing, transform); 121 compose_Zpivot_transform_L(angleZ, transform); 122 return transform; 123 } 124 125 static double* 126 compose_ZXpivot_transform_L 127 (const double angleZ, 128 const double angleX, 129 const double spacing, 130 double accum[12]) 131 { 132 ASSERT(accum); 133 compose_Xpivot_transform_L(angleX, spacing, accum); 134 compose_Zpivot_transform_L(angleZ, accum); 135 return accum; 136 } 137 138 static double* 139 compose_pivot_transform_L(const struct pivot_data* pivot, double accum[12]) { 140 ASSERT(pivot && accum); 141 switch (pivot->pivot.type) { 142 case PIVOT_SINGLE_AXIS: { 143 ASSERT(pivot->angleZ == 0); 144 compose_Xpivot_transform_L(pivot->angleX, 0, accum); 145 break; 146 } 147 case PIVOT_TWO_AXIS: { 148 compose_ZXpivot_transform_L( 149 pivot->angleZ, pivot->angleX, pivot->pivot.data.pivot2.spacing, accum); 150 break; 151 } 152 default: FATAL("Unreachable code.\n"); break; 153 } 154 return accum; 155 } 156 157 static double* 158 get_pivot_transform(const struct pivot_data* pivot, double transform[12]) 159 { 160 ASSERT(pivot && transform); 161 switch (pivot->pivot.type) { 162 case PIVOT_SINGLE_AXIS: { 163 ASSERT(pivot->angleZ == 0); 164 get_Xpivot_transform(pivot->angleX, 0, transform); 165 break; 166 } 167 case PIVOT_TWO_AXIS: { 168 get_ZXpivot_transform( 169 pivot->angleZ, pivot->angleX, pivot->pivot.data.pivot2.spacing, transform); 170 break; 171 } 172 default: FATAL("Unreachable code.\n"); break; 173 } 174 return transform; 175 } 176 177 static double* 178 node_get_own_transform 179 (const struct sanim_node* node, 180 const int include_pivot, 181 double transform[12]) 182 { 183 ASSERT(node && node->data && transform); 184 if (include_pivot && node->data->pivot_data) { 185 double local[12]; 186 get_pivot_transform(node->data->pivot_data, transform); 187 d33_rotation(local, SPLIT3(node->data->rotations)); 188 d3_set(local + 9, node->data->translation); 189 d34_muld34(transform, local, transform); 190 } 191 else { 192 d33_rotation(transform, SPLIT3(node->data->rotations)); 193 d3_set(transform + 9, node->data->translation); 194 } 195 return transform; 196 } 197 198 static double* 199 compose_node_transform_L(const struct sanim_node* node, double accum[12]) { 200 double local[12]; 201 ASSERT(node && node->data && accum); 202 if (node->data->pivot_data) { 203 compose_pivot_transform_L(node->data->pivot_data, accum); 204 } 205 d33_rotation(local, SPLIT3(node->data->rotations)); 206 d3_set(local + 9, node->data->translation); 207 d34_muld34(accum, local, accum); 208 return accum; 209 } 210 211 static void 212 node_get_transform 213 (const struct sanim_node* node, 214 const int include_own_pivot, 215 double transform[12]) 216 { 217 const struct sanim_node* father; 218 ASSERT(node && node->data && transform); 219 father = node->data->father; 220 node_get_own_transform(node, include_own_pivot, transform); 221 while (father) { 222 compose_node_transform_L(father, transform); 223 father = father->data->father; 224 } 225 } 226 227 static void 228 compute_single_axis_angle 229 (const double ref_2D[2], 230 const double rotated_2D[2], 231 double* angle ) 232 { 233 double x, y; 234 ASSERT(ref_2D && rotated_2D && angle); 235 ASSERT(d2_is_normalized(rotated_2D)); 236 ASSERT(d2_is_normalized(ref_2D)); 237 /* in the YZ plane */ 238 y = d2_cross(ref_2D, rotated_2D); 239 x = d2_dot(ref_2D, rotated_2D); 240 *angle = atan2(y, x); 241 } 242 243 static res_T 244 pivot_solve_single_axis_sun 245 (struct sanim_node* node, 246 const double in_dir[3]) 247 { 248 double mat[12], inv[12]; 249 double local_in[3]; 250 double local_in_2D[2] = {0, 0}; 251 double rotated_n_2D[2] = {0, 0}; 252 const double* ref_normal_2D; 253 struct pivot_data* pivot_data; 254 ASSERT(node && node->data && in_dir); 255 pivot_data = node->data->pivot_data; 256 ASSERT(pivot_data); 257 ASSERT(pivot_data->pivot.type == PIVOT_SINGLE_AXIS); 258 ASSERT(pivot_data->tracking.policy == TRACKING_SUN); 259 ASSERT(d3_is_normalized(in_dir)); 260 261 ref_normal_2D = pivot_data->pivot.data.pivot1.ref_normal + 1; 262 ASSERT(d2_is_normalized(ref_normal_2D)); 263 264 /* get in_dir in local space */ 265 node_get_transform(node, 0, mat); 266 d33_transpose(inv, mat); /* no scale factors: inverse is transpose */ 267 d33_muld3(local_in, inv, in_dir); 268 269 /* solve in the YZ plane */ 270 if (d2_normalize(local_in_2D, local_in + 1) < 0.25) { 271 /* not really in the YZ-plane */ 272 return RES_BAD_ARG; 273 } 274 275 /* rotated_n = -local_in */ 276 d2_muld(rotated_n_2D, local_in_2D, -1); 277 278 compute_single_axis_angle(ref_normal_2D, rotated_n_2D, &pivot_data->angleX); 279 return RES_OK; 280 } 281 282 static INLINE res_T 283 pivot_solve_single_axis_line(struct sanim_node* node, const double in_dir[3]) 284 { 285 double mat[12], inv[9]; 286 double local_in[3], local_target[3]; 287 double rotated_n_2D[2] = {0, 0}; 288 double local_out_2D[2] = {0, 0}; 289 double local_in_2D[2] = {0, 0}; 290 double ref_point_2D[2] = {0, 0}; 291 const double* ref_normal_2D; 292 double* const local_target_2D = local_target + 1; 293 struct pivot_data* pivot_data; 294 double angle, previous_angle, delta; 295 double sign_dA, prev_sign_dA; 296 double kA; 297 double d1, d2; 298 int cpt = 0; 299 ASSERT(node && node->data && in_dir); 300 pivot_data = node->data->pivot_data; 301 ASSERT(pivot_data); 302 ASSERT(pivot_data->pivot.type == PIVOT_SINGLE_AXIS); 303 ASSERT(pivot_data->tracking.policy == TRACKING_POINT 304 || pivot_data->tracking.policy == TRACKING_NODE_TARGET); 305 ASSERT(d3_is_normalized(in_dir)); 306 307 ref_normal_2D = pivot_data->pivot.data.pivot1.ref_normal + 1; 308 ASSERT(pivot_data->pivot.data.pivot1.ref_normal[0] == 0); /* solve in YZ plane */ 309 ASSERT(d2_is_normalized(ref_normal_2D)); 310 d2_set(ref_point_2D, pivot_data->pivot.data.pivot1.ref_point + 1); 311 312 /* get in_dir in local space */ 313 node_get_transform(node, 0, mat); 314 d33_transpose(inv, mat); /* no scale factors: inverse is transpose */ 315 d33_muld3(local_in, inv, in_dir); 316 /* solve in the YZ plane */ 317 if (d2_normalize(local_in_2D, local_in + 1) < 0.25) { 318 /* not really in the YZ-plane */ 319 return RES_BAD_ARG; 320 } 321 322 /* get target point in local space */ 323 if (pivot_data->tracking.policy == TRACKING_POINT) { 324 if (pivot_data->tracking.data.point.target_is_local) { 325 d3_set(local_target, pivot_data->tracking.data.point.target); 326 } 327 else { 328 d3_sub(local_target, pivot_data->tracking.data.point.target, mat + 9); 329 d33_muld3(local_target, inv, local_target); 330 } 331 } 332 else { 333 double transform[12]; 334 const struct sanim_node* target 335 = node->data->pivot_data->tracking.data.node_target.tracked_node; 336 ASSERT(target && target->data); 337 ASSERT(pivot_data->tracking.policy == TRACKING_NODE_TARGET); 338 if (is_after_pivot(target)) return RES_BAD_ARG; 339 node_get_transform(target, 0, transform); 340 d3_sub(local_target, transform + 9, mat + 9); 341 d33_muld3(local_target, inv, local_target); 342 } 343 344 /* check if in, target_point and ref_point are compatible */ 345 d1 = d2_dot(local_target_2D, local_target_2D); /* in the YZ plane */ 346 d2 = d2_dot(ref_point_2D, ref_point_2D); 347 if (d1 <= d2) { 348 /* target in the pivot */ 349 return RES_BAD_ARG; 350 } 351 352 angle = 0; 353 prev_sign_dA = 0; 354 kA = 0.9; 355 do { 356 double pivot[4]; 357 /* compute 2D normal after rotation */ 358 d2_sub(local_out_2D, local_target_2D, ref_point_2D); 359 if (d2_normalize(local_out_2D, local_out_2D) < 0.25) { 360 /* not really in the YZ-plane */ 361 return RES_BAD_ARG; 362 } 363 364 /* rotated_n = bisectrix of local_in and out_dir */ 365 d2_sub(rotated_n_2D, local_out_2D, local_in_2D); 366 if (d2_normalize(rotated_n_2D, rotated_n_2D) < 1e-4) { 367 /* tangent rays */ 368 return RES_BAD_ARG; 369 } 370 371 previous_angle = angle; 372 compute_single_axis_angle(ref_normal_2D, rotated_n_2D, &angle); 373 if (fabs(previous_angle - angle) > PI) { 374 previous_angle = (angle > 0) ? 2 * PI : -2 * PI; 375 } 376 377 delta = previous_angle - angle; 378 if (fabs(delta) < 1e-7 || ++cpt > 10) 379 break; 380 381 if (d2) { 382 /* only if ref_point is not the rotation point 383 * the heuristic is to amortize algorithm's oscillations */ 384 sign_dA = sign(previous_angle - angle); 385 if (prev_sign_dA != sign_dA) 386 kA *= 0.9; 387 else 388 kA *= 1 / 0.9; 389 angle = previous_angle + kA * (angle - previous_angle); 390 prev_sign_dA = sign_dA; 391 } 392 393 /* update ref_point */ 394 d22_rotation(pivot, angle); 395 d22_muld2(ref_point_2D, pivot, pivot_data->pivot.data.pivot1.ref_point + 1); 396 /* no d3_add(ref_point, ref_point, pivot + 9) as pivot has no offset to add */ 397 } while (1); 398 399 pivot_data->angleX = angle; 400 return RES_OK; 401 } 402 403 static INLINE res_T 404 pivot_solve_single_axis_dir 405 (struct sanim_node* node, 406 const double in_dir[3]) 407 { 408 double mat[12], inv[12]; 409 double local_in[3], local_out[3]; 410 double local_in_2D[2] = {0, 0}; 411 double rotated_n_2D[2] = {0, 0}; 412 double local_out_2D[2] = {0, 0}; 413 const double* ref_normal_2D; 414 struct pivot_data* pivot_data; 415 ASSERT(node && node->data && in_dir); 416 pivot_data = node->data->pivot_data; 417 ASSERT(pivot_data); 418 ASSERT(pivot_data->pivot.type == PIVOT_SINGLE_AXIS); 419 ASSERT(pivot_data->tracking.policy == TRACKING_OUT_DIR); 420 ASSERT(d3_is_normalized(in_dir)); 421 ASSERT(d3_is_normalized(pivot_data->tracking.data.out_dir.u)); 422 423 ref_normal_2D = pivot_data->pivot.data.pivot1.ref_normal + 1; 424 ASSERT(pivot_data->pivot.data.pivot1.ref_normal[0] == 0); /* solve in YZ plane */ 425 ASSERT(d2_is_normalized(ref_normal_2D)); 426 427 /* get in_dir and out_dir in local space */ 428 node_get_transform(node, 0, mat); 429 d33_transpose(inv, mat); /* no scale factors: inverse is transpose */ 430 d33_muld3(local_in, inv, in_dir); 431 d33_muld3(local_out, inv, pivot_data->tracking.data.out_dir.u); 432 433 /* solve in the YZ plane */ 434 if (d2_normalize(local_in_2D, local_in + 1) < 0.25) { 435 /* not really in the YZ-plane */ 436 return RES_BAD_ARG; 437 } 438 if (d2_normalize(local_out_2D, local_out + 1) < 0.25) { 439 /* not really in the YZ-plane */ 440 return RES_BAD_ARG; 441 } 442 443 /* rotated_n = bisectrix of local_in and out_dir */ 444 d2_sub(rotated_n_2D, local_out_2D, local_in_2D); 445 if (d2_normalize(rotated_n_2D, rotated_n_2D) < 1e-4) { 446 /* tangent rays */ 447 return RES_BAD_ARG; 448 } 449 450 compute_single_axis_angle(ref_normal_2D, rotated_n_2D, &pivot_data->angleX); 451 return RES_OK; 452 } 453 454 static INLINE res_T 455 pivot_solve_single_axis 456 (struct sanim_node* node, 457 const double in_dir[3]) 458 { 459 res_T res = RES_OK; 460 ASSERT(node && in_dir); 461 ASSERT(node->data->pivot_data); 462 ASSERT(node->data->pivot_data->pivot.type == PIVOT_SINGLE_AXIS); 463 464 switch (node->data->pivot_data->tracking.policy) { 465 case TRACKING_SUN: 466 res = pivot_solve_single_axis_sun(node, in_dir); 467 break; 468 case TRACKING_POINT: 469 case TRACKING_NODE_TARGET: 470 /* track the X line that includes ref_point */ 471 res = pivot_solve_single_axis_line(node, in_dir); 472 break; 473 case TRACKING_OUT_DIR: 474 res = pivot_solve_single_axis_dir(node, in_dir); 475 break; 476 default: FATAL("Unreachable code.\n"); break; 477 } 478 ASSERT(node->data->pivot_data->angleZ == 0); 479 return res; 480 } 481 482 static void 483 compute_two_axis_angles 484 (const double rotated_n[3], 485 double* angleX, 486 double* angleZ) 487 { 488 /* ref normal is <0,1,0> */ 489 ASSERT(rotated_n && angleX && angleZ); 490 ASSERT(d3_is_normalized(rotated_n)); 491 if (fabs(rotated_n[2]) >= 1) { 492 *angleX = 0.5 * sign(rotated_n[2]) * PI; 493 *angleZ = 0; 494 return; 495 } 496 *angleX = asin(rotated_n[2]); 497 *angleZ = atan2(-rotated_n[0], rotated_n[1]); 498 } 499 500 static INLINE res_T 501 pivot_solve_two_axis_sun 502 (struct sanim_node* node, 503 const double in_dir[3]) 504 { 505 double mat[12], inv[12]; 506 double local_in[3], rotated_n[3]; 507 struct pivot_data* pivot_data; 508 ASSERT(node && node->data && in_dir); 509 pivot_data = node->data->pivot_data; 510 ASSERT(pivot_data); 511 ASSERT(pivot_data->pivot.type == PIVOT_TWO_AXIS); 512 ASSERT(pivot_data->tracking.policy == TRACKING_SUN); 513 ASSERT(d3_is_normalized(in_dir)); 514 515 /* get in_dir in local space */ 516 node_get_transform(node, 0, mat); 517 d33_transpose(inv, mat); /* no scale factors: inverse is transpose */ 518 d33_muld3(local_in, inv, in_dir); 519 ASSERT(d3_is_normalized(local_in)); 520 521 /* rotated_n = -local_in */ 522 d3_muld(rotated_n, local_in, -1); 523 524 compute_two_axis_angles(rotated_n, &pivot_data->angleX, &pivot_data->angleZ); 525 return RES_OK; 526 } 527 528 static INLINE res_T 529 pivot_solve_two_axis_point 530 (struct sanim_node* node, 531 const double in_dir[3]) 532 { 533 double mat[12], inv[9]; 534 double local_in[3], rotated_n[3], local_out[3], local_target[3], ref_point[3]; 535 double angleX, angleZ, previous_angleX, previous_angleZ, delta; 536 double sign_dX, sign_dZ, prev_sign_dX, prev_sign_dZ; 537 double kX, kZ; 538 double d1, d2; 539 struct pivot_data* pivot_data; 540 int cpt = 0; 541 ASSERT(node && node->data && in_dir); 542 pivot_data = node->data->pivot_data; 543 ASSERT(pivot_data); 544 ASSERT(pivot_data->pivot.type == PIVOT_TWO_AXIS); 545 ASSERT(pivot_data->tracking.policy == TRACKING_POINT 546 || pivot_data->tracking.policy == TRACKING_NODE_TARGET); 547 ASSERT(d3_is_normalized(in_dir)); 548 549 d3_set(ref_point, pivot_data->pivot.data.pivot2.ref_point); 550 ref_point[1] += pivot_data->pivot.data.pivot2.spacing; 551 552 /* get in_dir in local space */ 553 node_get_transform(node, 0, mat); 554 d33_transpose(inv, mat); /* no scale factors: inverse is transpose */ 555 d33_muld3(local_in, inv, in_dir); 556 ASSERT(d3_is_normalized(local_in)); 557 558 /* get target point in local space */ 559 if (pivot_data->tracking.policy == TRACKING_POINT) { 560 if (pivot_data->tracking.data.point.target_is_local) { 561 d3_set(local_target, pivot_data->tracking.data.point.target); 562 } 563 else { 564 d3_sub(local_target, pivot_data->tracking.data.point.target, mat + 9); 565 d33_muld3(local_target, inv, local_target); 566 } 567 } 568 else { 569 double transform[12]; 570 const struct sanim_node* target 571 = node->data->pivot_data->tracking.data.node_target.tracked_node; 572 ASSERT(target && target->data); 573 ASSERT(pivot_data->tracking.policy == TRACKING_NODE_TARGET); 574 if (is_after_pivot(target)) return RES_BAD_ARG; 575 node_get_transform(target, 0, transform); 576 d3_sub(local_target, transform + 9, mat + 9); 577 d33_muld3(local_target, inv, local_target); 578 } 579 580 /* check if in, target_point and ref_point are compatible */ 581 d1 = d3_dot(local_target, local_target); 582 d2 = d3_dot(ref_point, ref_point); 583 if (d1 <= d2) { 584 /* target in the pivot */ 585 return RES_BAD_ARG; 586 } 587 588 angleX = angleZ = 0; 589 prev_sign_dX = prev_sign_dZ = 0; 590 kX = kZ = 0.9; 591 do { 592 double pivot[12]; 593 /* compute rotated_n */ 594 d3_sub(local_out, local_target, ref_point); 595 d3_normalize(local_out, local_out); 596 597 /* rotated_n = bisectrix of local_in and out_dir */ 598 d3_sub(rotated_n, local_out, local_in); 599 if (d3_normalize(rotated_n, rotated_n) < 1e-4) { 600 /* tangent rays */ 601 return RES_BAD_ARG; 602 } 603 604 previous_angleX = angleX; 605 previous_angleZ = angleZ; 606 compute_two_axis_angles(rotated_n, &angleX, &angleZ); 607 if (fabs(previous_angleX - angleX) > PI) { 608 previous_angleX = (angleX > 0) ? 2 * PI : -2 * PI; 609 } 610 if (fabs(previous_angleZ - angleZ) > PI) { 611 previous_angleZ += (angleZ > 0) ? 2 * PI : -2 * PI; 612 } 613 delta = MMAX(fabs(previous_angleX - angleX), fabs(previous_angleZ - angleZ)); 614 if (delta < 1e-7 || ++cpt > 10) 615 break; 616 617 if (d2) { 618 /* only if ref_point is not the rotation point 619 * the heuristic is to amortize algorithm's oscillations */ 620 sign_dX = sign(previous_angleX - angleX); 621 sign_dZ = sign(previous_angleZ - angleZ); 622 if (prev_sign_dX != sign_dX) 623 kX *= 0.9; 624 else 625 kX *= 1 / 0.9; 626 angleX = previous_angleX + kX * (angleX - previous_angleX); 627 if (prev_sign_dZ != sign_dZ) 628 kZ *= 0.9; 629 else 630 kZ *= 1 / 0.9; 631 angleZ = previous_angleZ + kZ * (angleZ - previous_angleZ); 632 prev_sign_dX = sign_dX; 633 prev_sign_dZ = sign_dZ; 634 } 635 636 get_ZXpivot_transform( 637 angleZ, angleX, pivot_data->pivot.data.pivot2.spacing, pivot); 638 /* update ref_point */ 639 d33_muld3(ref_point, pivot, pivot_data->pivot.data.pivot2.ref_point); 640 d3_add(ref_point, ref_point, pivot + 9); 641 } while (1); 642 643 pivot_data->angleX = angleX; 644 pivot_data->angleZ = angleZ; 645 return RES_OK; 646 } 647 648 static INLINE res_T 649 pivot_solve_two_axis_dir 650 (struct sanim_node* node, 651 const double in_dir[3]) 652 { 653 double mat[12], inv[12]; 654 double local_in[3], rotated_n[3], local_out[3]; 655 struct pivot_data* pivot_data; 656 ASSERT(node && node->data && in_dir); 657 pivot_data = node->data->pivot_data; 658 ASSERT(pivot_data); 659 ASSERT(pivot_data->pivot.type == PIVOT_TWO_AXIS); 660 ASSERT(pivot_data->tracking.policy == TRACKING_OUT_DIR); 661 ASSERT(d3_is_normalized(in_dir)); 662 ASSERT(d3_is_normalized(pivot_data->tracking.data.out_dir.u)); 663 664 /* get in_dir and out_dir in local space */ 665 node_get_transform(node, 0, mat); 666 d33_transpose(inv, mat); /* no scale factors: inverse is transpose */ 667 d33_muld3(local_in, inv, in_dir); 668 d33_muld3(local_out, inv, pivot_data->tracking.data.out_dir.u); 669 670 /* rotated_n = bisectrix of local_in and out_dir */ 671 d3_sub(rotated_n, local_out, local_in); 672 if (d3_normalize(rotated_n, rotated_n) < 1e-4) { 673 /* tangent rays */ 674 return RES_BAD_ARG; 675 } 676 677 compute_two_axis_angles(rotated_n, &pivot_data->angleX, &pivot_data->angleZ); 678 return RES_OK; 679 } 680 681 static INLINE res_T 682 pivot_solve_two_axis 683 (struct sanim_node* node, 684 const double in_dir[3]) 685 { 686 res_T res = RES_OK; 687 ASSERT(node && in_dir); 688 ASSERT(node->data->pivot_data); 689 ASSERT(node->data->pivot_data->pivot.type == PIVOT_TWO_AXIS); 690 691 switch (node->data->pivot_data->tracking.policy) { 692 case TRACKING_SUN: 693 res = pivot_solve_two_axis_sun(node, in_dir); 694 break; 695 case TRACKING_POINT: 696 case TRACKING_NODE_TARGET: 697 res = pivot_solve_two_axis_point(node, in_dir); 698 break; 699 case TRACKING_OUT_DIR: 700 res = pivot_solve_two_axis_dir(node, in_dir); 701 break; 702 default: FATAL("Unreachable code.\n"); break; 703 } 704 return res; 705 } 706 707 static INLINE res_T 708 copy_and_normalise_pivot_data 709 (struct pivot_data* dest, 710 const struct sanim_pivot* pivot, 711 const struct sanim_tracking* tracking) 712 { 713 dest->pivot.type = pivot->type; 714 switch (pivot->type) { 715 case PIVOT_SINGLE_AXIS: 716 if (!d3_normalize( 717 dest->pivot.data.pivot1.ref_normal, pivot->data.pivot1.ref_normal)) 718 return RES_BAD_ARG; 719 if (dest->pivot.data.pivot1.ref_normal[0]) 720 /* ref_normal not in the YZ plane */ 721 return RES_BAD_ARG; 722 d3_set(dest->pivot.data.pivot1.ref_point, pivot->data.pivot1.ref_point); 723 break; 724 case PIVOT_TWO_AXIS: 725 if (pivot->data.pivot2.spacing < 0) 726 return RES_BAD_ARG; 727 d3_set(dest->pivot.data.pivot2.ref_point, pivot->data.pivot2.ref_point); 728 dest->pivot.data.pivot2.spacing = pivot->data.pivot2.spacing; 729 break; 730 default: FATAL("Unreachable code.\n"); break; 731 } 732 dest->tracking.policy = tracking->policy; 733 switch (tracking->policy) { 734 case TRACKING_SUN: 735 /* nothing to be copied */ 736 break; 737 case TRACKING_POINT: 738 d3_set(dest->tracking.data.point.target, tracking->data.point.target); 739 dest->tracking.data.point.target_is_local 740 = tracking->data.point.target_is_local; 741 break; 742 case TRACKING_NODE_TARGET: 743 dest->tracking.data.node_target.tracked_node 744 = tracking->data.node_target.tracked_node; 745 break; 746 case TRACKING_OUT_DIR: 747 if (!d3_normalize(dest->tracking.data.out_dir.u, tracking->data.out_dir.u)) 748 return RES_BAD_ARG; 749 break; 750 default: FATAL("Unreachable code.\n"); break; 751 } 752 return RES_OK; 753 } 754 755 static res_T 756 node_solve_pivot 757 (struct sanim_node* node, 758 const double in_dir[3]) 759 { 760 ASSERT(node && node->data && in_dir && node->data->pivot_data); 761 ASSERT(d3_is_normalized(in_dir)); 762 763 switch (node->data->pivot_data->pivot.type) { 764 case PIVOT_SINGLE_AXIS: 765 return pivot_solve_single_axis(node, in_dir); 766 break; 767 case PIVOT_TWO_AXIS: 768 return pivot_solve_two_axis(node, in_dir); 769 break; 770 default: FATAL("Unreachable code.\n"); break; 771 } 772 } 773 774 static double* 775 compose_Xpivot_transform_R 776 (const double angle, 777 const double spacing, 778 double accum[12]) 779 { 780 double pivot[12]; 781 ASSERT(accum); 782 get_Xpivot_transform(angle, spacing, pivot); 783 d34_muld34(accum, accum, pivot); 784 return accum; 785 } 786 787 static double* 788 compose_Zpivot_transform_R(const double angle, double accum[12]) { 789 double pivot[12]; 790 ASSERT(accum); 791 get_Zpivot_transform(angle, pivot); 792 d34_muld34(accum, accum, pivot); 793 return accum; 794 } 795 796 static double* 797 compose_ZXpivot_transform_R 798 (const double angleZ, 799 const double angleX, 800 const double spacing, 801 double accum[12]) 802 { 803 ASSERT(accum); 804 compose_Zpivot_transform_R(angleZ, accum); 805 compose_Xpivot_transform_R(angleX, spacing, accum); 806 return accum; 807 } 808 809 static double* 810 compose_pivot_transform_R(double accum[12], const struct pivot_data* pivot) { 811 ASSERT(pivot && accum); 812 switch (pivot->pivot.type) { 813 case PIVOT_SINGLE_AXIS: { 814 ASSERT(pivot->angleZ == 0); 815 compose_Xpivot_transform_R(pivot->angleX, 0, accum); 816 break; 817 } 818 case PIVOT_TWO_AXIS: { 819 compose_ZXpivot_transform_R( 820 pivot->angleZ, pivot->angleX, pivot->pivot.data.pivot2.spacing, accum); 821 break; 822 } 823 default: FATAL("Unreachable code.\n"); break; 824 } 825 return accum; 826 } 827 828 static double* 829 compose_node_transform_R(double accum[12], const struct sanim_node* node) 830 { 831 double local[12]; 832 ASSERT(node && node->data && accum); 833 d33_rotation(local, SPLIT3(node->data->rotations)); 834 d3_set(local + 9, node->data->translation); 835 d34_muld34(accum, accum, local); 836 if (node->data->pivot_data) { 837 compose_pivot_transform_R(accum, node->data->pivot_data); 838 } 839 return accum; 840 } 841 842 static res_T 843 visit_tree 844 (struct sanim_node* node, 845 const double in_dir[3], 846 void* data, 847 res_T(*visitor)( 848 const struct sanim_node* n, const double transform[12], void* data), 849 const double affine_transform[12]) 850 { 851 size_t count, i; 852 struct sanim_node* const* children; 853 double transform[12]; 854 res_T res = RES_OK; 855 ASSERT(node && node->data && visitor); 856 ASSERT(!in_dir || d3_is_normalized(in_dir)); 857 858 if (in_dir && node->data->pivot_data) { 859 res = node_solve_pivot(node, in_dir); 860 if (res != RES_OK) return res; 861 } 862 863 d33_set(transform, affine_transform); 864 d3_set(transform+9, affine_transform+9); 865 compose_node_transform_R(transform, node); 866 res = visitor(node, transform, data); 867 if (res != RES_OK) return res; 868 869 count = darray_children_size_get(&node->data->children); 870 children = darray_children_data_get(&node->data->children); 871 for (i = 0; i < count; i++) { 872 struct sanim_node* child = children[i]; 873 res = visit_tree(child, in_dir, data, visitor, transform); 874 if (res != RES_OK) return res; 875 } 876 return RES_OK; 877 } 878 879 /******************************************************************************* 880 * Exported sanim_node functions 881 ******************************************************************************/ 882 res_T 883 sanim_node_add_child 884 (struct sanim_node* father, 885 struct sanim_node* child) 886 { 887 res_T res = RES_OK; 888 889 if (!father || !child 890 || !father->data || !child->data 891 ) return RES_BAD_ARG; 892 if (child->data->father) return RES_BAD_ARG; 893 if (is_ancestor(father, child)) return RES_BAD_ARG; 894 if (child->data->pivot_data && is_after_pivot(father)) return RES_BAD_ARG; 895 896 child->data->father = father; 897 res = darray_children_push_back(&father->data->children, &child); 898 if (res != RES_OK) { 899 goto error; 900 } 901 902 exit: 903 return res; 904 error: 905 if (child->data) { 906 child->data = NULL; 907 } 908 goto exit; 909 } 910 911 res_T 912 sanim_node_initialize 913 (struct mem_allocator* allocator, 914 struct sanim_node* node) 915 { 916 res_T res = RES_OK; 917 918 if (!allocator || !node) return RES_BAD_ARG; 919 node->data = MEM_CALLOC(allocator, 1, sizeof(struct node_data)); 920 if (!node->data) { 921 res = RES_MEM_ERR; 922 goto error; 923 } 924 925 darray_children_init(allocator, &node->data->children); 926 node->data->allocator = allocator; 927 928 exit: 929 return res; 930 error: 931 if (node->data) { 932 darray_children_release(&node->data->children); 933 node->data = NULL; 934 } 935 goto exit; 936 } 937 938 res_T 939 sanim_node_initialize_pivot 940 (struct mem_allocator* allocator, 941 const struct sanim_pivot* pivot, 942 const struct sanim_tracking* tracking, 943 struct sanim_node* node) 944 { 945 res_T res = RES_OK; 946 947 if (!allocator || !node || !pivot || !tracking) return RES_BAD_ARG; 948 res = sanim_node_initialize(allocator, node); 949 if (res != RES_OK) goto error; 950 951 node->data->pivot_data = MEM_CALLOC(allocator, 1, sizeof(struct pivot_data)); 952 if (!node->data->pivot_data) { 953 res = RES_MEM_ERR; 954 goto error; 955 } 956 957 res = copy_and_normalise_pivot_data(node->data->pivot_data, pivot, tracking); 958 if (res != RES_OK) goto error; 959 960 exit: 961 return res; 962 error: 963 sanim_node_release(node); 964 goto exit; 965 } 966 967 res_T 968 sanim_node_copy_initialize 969 (struct mem_allocator* allocator, 970 const struct sanim_node* src, 971 struct sanim_node* dst) 972 { 973 res_T res = RES_OK; 974 975 if (!allocator || !src || !dst) return RES_BAD_ARG; 976 res = sanim_node_initialize(allocator, dst); 977 if (res != RES_OK) goto error; 978 979 if (src->data->pivot_data) { 980 dst->data->pivot_data = MEM_CALLOC(allocator, 1, sizeof(struct pivot_data)); 981 if (!dst->data->pivot_data) { 982 res = RES_MEM_ERR; 983 goto error; 984 } 985 dst->data->pivot_data->angleX = src->data->pivot_data->angleX; 986 dst->data->pivot_data->angleZ = src->data->pivot_data->angleZ; 987 dst->data->pivot_data->pivot = src->data->pivot_data->pivot; 988 dst->data->pivot_data->tracking = src->data->pivot_data->tracking; 989 } 990 d3_set(dst->data->rotations, src->data->rotations); 991 d3_set(dst->data->translation, src->data->translation); 992 993 exit: 994 return res; 995 error: 996 sanim_node_release(dst); 997 goto exit; 998 } 999 1000 res_T 1001 sanim_node_is_initialized 1002 (const struct sanim_node* node, 1003 int* initialized) 1004 { 1005 if (!node || !initialized) return RES_BAD_ARG; 1006 *initialized = (node->data != NULL); 1007 return RES_OK; 1008 } 1009 1010 res_T 1011 sanim_node_solve_pivot 1012 (struct sanim_node* node, 1013 const double in_dir[3]) 1014 { 1015 double dir[3]; 1016 if (!node || !node->data || !in_dir) return RES_BAD_ARG; 1017 if (!node->data->pivot_data) return RES_BAD_ARG; 1018 if (!d3_normalize(dir, in_dir)) return RES_BAD_ARG; 1019 1020 return node_solve_pivot(node, dir); 1021 } 1022 1023 res_T 1024 sanim_node_visit_tree 1025 (struct sanim_node* node, 1026 const double in_dir[3], 1027 void* data, 1028 res_T(*visitor) 1029 (const struct sanim_node* n, const double transform[12], void* data)) 1030 { 1031 double dir[3]; 1032 double transform[12]; 1033 size_t count, i; 1034 struct sanim_node* const* children; 1035 res_T res = RES_OK; 1036 if (!node || !node->data || !visitor) return RES_BAD_ARG; 1037 if (in_dir && !d3_normalize(dir, in_dir)) return RES_BAD_ARG; 1038 1039 if (in_dir && node->data->pivot_data) { 1040 res = node_solve_pivot(node, dir); 1041 if (res != RES_OK) return res; 1042 } 1043 1044 /* node transform, including possible ascendants */ 1045 node_get_transform(node, 1, transform); 1046 res = visitor(node, transform, data); 1047 if (res != RES_OK) return res; 1048 1049 count = darray_children_size_get(&node->data->children); 1050 children = darray_children_data_get(&node->data->children); 1051 for (i = 0; i < count; i++) { 1052 struct sanim_node* child = children[i]; 1053 res = visit_tree(child, in_dir ? dir : NULL, data, visitor, transform); 1054 if (res != RES_OK) return res; 1055 } 1056 return RES_OK; 1057 } 1058 1059 res_T 1060 sanim_node_search_tree 1061 (const struct sanim_node* node, 1062 void* data, 1063 res_T(*cmp)( 1064 const struct sanim_node* n, void* data, int* found), 1065 int* found) 1066 { 1067 size_t count, i; 1068 struct sanim_node* const* children; 1069 res_T res = RES_OK; 1070 if (!node || !node->data || !cmp || !found) return RES_BAD_ARG; 1071 1072 res = cmp(node, data, found); 1073 if (*found || res != RES_OK) return res; 1074 1075 count = darray_children_size_get(&node->data->children); 1076 children = darray_children_data_get(&node->data->children); 1077 for (i = 0; i < count; i++) { 1078 struct sanim_node* child = children[i]; 1079 res = sanim_node_search_tree(child, data, cmp, found); 1080 if (*found || res != RES_OK) return res; 1081 } 1082 return RES_OK; 1083 } 1084 1085 res_T 1086 sanim_node_track_me 1087 (const struct sanim_node* node, 1088 struct sanim_tracking* tracking) 1089 { 1090 if (!node || !node->data || !tracking) return RES_BAD_ARG; 1091 tracking->policy = TRACKING_NODE_TARGET; 1092 tracking->data.node_target.tracked_node = node; 1093 return RES_OK; 1094 } 1095 1096 res_T 1097 sanim_node_release 1098 (struct sanim_node* node) 1099 { 1100 if (!node) return RES_BAD_ARG; 1101 if (node->data) { 1102 darray_children_release(&node->data->children); 1103 if (node->data->pivot_data) { 1104 MEM_RM(node->data->allocator, node->data->pivot_data); 1105 } 1106 MEM_RM(node->data->allocator, node->data); 1107 node->data = NULL; 1108 } 1109 return RES_OK; 1110 } 1111 1112 res_T 1113 sanim_node_set_translation 1114 (struct sanim_node* node, 1115 const double translation[3]) 1116 { 1117 if (!node || !node->data || !translation) return RES_BAD_ARG; 1118 d3_set(node->data->translation, translation); 1119 return RES_OK; 1120 } 1121 1122 res_T 1123 sanim_node_get_translation 1124 (const struct sanim_node* node, 1125 double translation[3]) 1126 { 1127 if (!node || !node->data || !translation) return RES_BAD_ARG; 1128 d3_set(translation, node->data->translation); 1129 return RES_OK; 1130 } 1131 1132 res_T 1133 sanim_node_set_rotations 1134 (struct sanim_node* node, 1135 const double rotations[3]) 1136 { 1137 if (!node || !node->data || !rotations) return RES_BAD_ARG; 1138 d3_set(node->data->rotations, rotations); 1139 return RES_OK; 1140 } 1141 1142 res_T 1143 sanim_node_get_rotations 1144 (const struct sanim_node* node, 1145 double rotations[3]) 1146 { 1147 if (!node || !node->data || !rotations) return RES_BAD_ARG; 1148 d3_set(rotations, node->data->rotations); 1149 return RES_OK; 1150 } 1151 1152 res_T 1153 sanim_node_get_transform(const struct sanim_node* node, double transform[12]) 1154 { 1155 if (!node || !node->data || !transform) 1156 return RES_BAD_ARG; 1157 node_get_transform(node, 1, transform); 1158 return RES_OK; 1159 } 1160 1161 res_T 1162 sanim_node_get_father 1163 (const struct sanim_node* node, 1164 const struct sanim_node** father) 1165 { 1166 if (!node || !father || !node->data) 1167 return RES_BAD_ARG; 1168 *father = node->data->father; 1169 return RES_OK; 1170 } 1171 1172 res_T 1173 sanim_node_get_children_count 1174 (const struct sanim_node* node, 1175 size_t* count) 1176 { 1177 if (!node || !count || !node->data) 1178 return RES_BAD_ARG; 1179 *count = darray_children_size_get(&node->data->children); 1180 return RES_OK; 1181 } 1182 1183 res_T 1184 sanim_node_get_child 1185 (const struct sanim_node* node, 1186 const size_t idx, 1187 struct sanim_node** child) 1188 { 1189 struct sanim_node* const* children; 1190 if (!node || !child || !node->data) 1191 return RES_BAD_ARG; 1192 if (idx >= darray_children_size_get(&node->data->children)) 1193 return RES_BAD_ARG; 1194 children = darray_children_cdata_get(&node->data->children); 1195 *child = children[idx]; 1196 return RES_OK; 1197 } 1198 1199 res_T 1200 sanim_node_is_pivot 1201 (const struct sanim_node* node, 1202 int* pivot) 1203 { 1204 if (!node || !pivot || !node->data) return RES_BAD_ARG; 1205 *pivot = (NULL != node->data->pivot_data); 1206 return RES_OK; 1207 }