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