00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "eggXfmSAnim.h"
00016 #include "eggSAnimData.h"
00017 #include "eggXfmAnimData.h"
00018 #include "eggParameters.h"
00019 #include "config_egg.h"
00020
00021 #include "indent.h"
00022 #include "compose_matrix.h"
00023 #include "dcast.h"
00024
00025 #include <math.h>
00026
00027 TypeHandle EggXfmSAnim::_type_handle;
00028
00029
00030
00031
00032
00033
00034 const string EggXfmSAnim::_standard_order_legacy = "sphrt";
00035 const string EggXfmSAnim::_standard_order_hpr_fix = "srpht";
00036
00037
00038
00039
00040
00041
00042
00043
00044 EggXfmSAnim::
00045 EggXfmSAnim(const EggXfmAnimData &convert_from)
00046 : EggGroupNode(convert_from.get_name())
00047 {
00048 _has_fps = false;
00049 _coordsys = convert_from.get_coordinate_system();
00050
00051 if (convert_from.has_order()) {
00052 set_order(convert_from.get_order());
00053 }
00054 if (convert_from.has_fps()) {
00055 set_fps(convert_from.get_fps());
00056 }
00057
00058 const string &contents = convert_from.get_contents();
00059 for (int col = 0; col < convert_from.get_num_cols(); col++) {
00060 EggSAnimData *sanim = new EggSAnimData(contents.substr(col, 1));
00061 add_child(sanim);
00062 for (int row = 0; row < convert_from.get_num_rows(); row++) {
00063 sanim->add_data(convert_from.get_value(row, col));
00064 }
00065 }
00066 }
00067
00068
00069
00070
00071
00072
00073
00074 void EggXfmSAnim::
00075 optimize() {
00076 iterator ci = begin();
00077 while (ci != end()) {
00078 iterator ci_next = ci;
00079 ++ci_next;
00080
00081 if ((*ci)->is_of_type(EggSAnimData::get_class_type())) {
00082 EggSAnimData *sanim = DCAST(EggSAnimData, *ci);
00083 sanim->optimize();
00084
00085 if (sanim->get_num_rows() == 1) {
00086
00087
00088 double value = sanim->get_value(0);
00089 double default_value;
00090 if (sanim->has_name() && strchr("ijk", sanim->get_name()[0]) != NULL) {
00091 default_value = 1.0;
00092 } else {
00093 default_value = 0.0;
00094 }
00095
00096 if (fabs(value - default_value) < egg_parameters->_table_threshold) {
00097
00098
00099 erase(ci);
00100 }
00101 }
00102 }
00103
00104 ci = ci_next;
00105 }
00106 }
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117 void EggXfmSAnim::
00118 optimize_to_standard_order() {
00119 if (get_order() != get_standard_order()) {
00120 normalize_by_rebuilding();
00121 }
00122 optimize();
00123 }
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134 void EggXfmSAnim::
00135 normalize() {
00136 if (get_order() != get_standard_order()) {
00137
00138
00139
00140 normalize_by_rebuilding();
00141
00142 } else {
00143
00144
00145
00146 normalize_by_expanding();
00147 }
00148 }
00149
00150
00151
00152
00153
00154
00155
00156 bool EggXfmSAnim::
00157 is_anim_matrix() const {
00158 return true;
00159 }
00160
00161
00162
00163
00164
00165
00166
00167 void EggXfmSAnim::
00168 write(ostream &out, int indent_level) const {
00169 test_under_integrity();
00170
00171 write_header(out, indent_level, "<Xfm$Anim_S$>");
00172
00173 if (has_fps()) {
00174 indent(out, indent_level + 2) << "<Scalar> fps { " << get_fps() << " }\n";
00175 }
00176
00177 if (has_order()) {
00178 indent(out, indent_level + 2)
00179 << "<Char*> order { " << get_order() << " }\n";
00180 }
00181
00182
00183
00184
00185
00186
00187 EggSAnimData *tables[num_matrix_components];
00188 memset(tables, 0, sizeof(EggSAnimData *) * num_matrix_components);
00189
00190 const_iterator ci;
00191 for (ci = begin(); ci != end(); ++ci) {
00192 EggNode *child = (*ci);
00193 if (child->is_of_type(EggSAnimData::get_class_type())) {
00194 EggSAnimData *sanim = DCAST(EggSAnimData, *ci);
00195
00196
00197 nassertv(sanim->get_name().length() == 1);
00198 char name = sanim->get_name()[0];
00199 char *p = (char *)strchr(matrix_component_letters, name);
00200 nassertv(p != (char *)NULL);
00201 if (p != (char *)NULL) {
00202 int index = p - matrix_component_letters;
00203 nassertv(tables[index] == (EggSAnimData *)NULL);
00204 tables[index] = sanim;
00205 }
00206 } else {
00207
00208 child->write(out, indent_level + 2);
00209 }
00210 }
00211
00212
00213 for (int i = 0; i < num_matrix_components; i++) {
00214 if (tables[i] != (EggSAnimData *)NULL) {
00215 tables[i]->write(out, indent_level + 2);
00216 }
00217 }
00218
00219 indent(out, indent_level) << "}\n";
00220 }
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231 void EggXfmSAnim::
00232 compose_with_order(LMatrix4d &mat,
00233 const LVecBase3d &scale,
00234 const LVecBase3d &shear,
00235 const LVecBase3d &hpr,
00236 const LVecBase3d &trans,
00237 const string &order,
00238 CoordinateSystem cs) {
00239
00240 mat = LMatrix4d::ident_mat();
00241
00242 bool reverse_roll = false;
00243
00244 if (order == "sphrt" && egg_support_old_anims) {
00245
00246
00247
00248 reverse_roll = true;
00249 }
00250
00251 string::const_iterator pi;
00252 for (pi = order.begin(); pi != order.end(); ++pi) {
00253 switch (*pi) {
00254 case 's':
00255 mat = mat * LMatrix4d::scale_shear_mat(scale, shear, cs);
00256 break;
00257
00258 case 'h':
00259 mat = mat * LMatrix4d::rotate_mat_normaxis(hpr[0], LVector3d::up(cs), cs);
00260 break;
00261
00262 case 'p':
00263 mat = mat * LMatrix4d::rotate_mat_normaxis(hpr[1], LVector3d::right(cs), cs);
00264 break;
00265
00266 case 'r':
00267 if (reverse_roll) {
00268 mat = mat * LMatrix4d::rotate_mat_normaxis(-hpr[2], LVector3d::forward(cs), cs);
00269 } else {
00270 mat = mat * LMatrix4d::rotate_mat_normaxis(hpr[2], LVector3d::forward(cs), cs);
00271 }
00272 break;
00273
00274 case 't':
00275 mat = mat * LMatrix4d::translate_mat(trans);
00276 break;
00277
00278 default:
00279 egg_cat.warning()
00280 << "Invalid letter in order string: " << *pi << "\n";
00281 }
00282 }
00283 }
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294 int EggXfmSAnim::
00295 get_num_rows() const {
00296 bool found_any = false;
00297 int min_rows = 1;
00298
00299 const_iterator ci;
00300 for (ci = begin(); ci != end(); ++ci) {
00301 if ((*ci)->is_of_type(EggSAnimData::get_class_type())) {
00302 EggSAnimData *sanim = DCAST(EggSAnimData, *ci);
00303 if (sanim->get_num_rows() > 1) {
00304 if (!found_any) {
00305 min_rows = sanim->get_num_rows();
00306
00307 } else {
00308 min_rows = min(min_rows, sanim->get_num_rows());
00309 }
00310 }
00311 }
00312 }
00313
00314 return min_rows;
00315 }
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328 void EggXfmSAnim::
00329 get_value(int row, LMatrix4d &mat) const {
00330 LVector3d scale(1.0, 1.0, 1.0);
00331 LVector3d shear(0.0, 0.0, 0.0);
00332 LVector3d hpr(0.0, 0.0, 0.0);
00333 LVector3d translate(0.0, 0.0, 0.0);
00334
00335 const_iterator ci;
00336 for (ci = begin(); ci != end(); ++ci) {
00337 if ((*ci)->is_of_type(EggSAnimData::get_class_type())) {
00338 EggSAnimData *sanim = DCAST(EggSAnimData, *ci);
00339
00340 if (sanim->get_num_rows() == 0) {
00341
00342
00343 break;
00344 }
00345
00346 double value;
00347 if (sanim->get_num_rows() == 1) {
00348 value = sanim->get_value(0);
00349 } else {
00350 nassertv(row < sanim->get_num_rows());
00351 value = sanim->get_value(row);
00352 }
00353
00354
00355 nassertv(sanim->get_name().length() == 1);
00356
00357 switch (sanim->get_name()[0]) {
00358 case 'i':
00359 scale[0] = value;
00360 break;
00361
00362 case 'j':
00363 scale[1] = value;
00364 break;
00365
00366 case 'k':
00367 scale[2] = value;
00368 break;
00369
00370 case 'a':
00371 shear[0] = value;
00372 break;
00373
00374 case 'b':
00375 shear[1] = value;
00376 break;
00377
00378 case 'c':
00379 shear[2] = value;
00380 break;
00381
00382 case 'h':
00383 hpr[0] = value;
00384 break;
00385
00386 case 'p':
00387 hpr[1] = value;
00388 break;
00389
00390 case 'r':
00391 hpr[2] = value;
00392 break;
00393
00394 case 'x':
00395 translate[0] = value;
00396 break;
00397
00398 case 'y':
00399 translate[1] = value;
00400 break;
00401
00402 case 'z':
00403 translate[2] = value;
00404 break;
00405
00406 default:
00407
00408 nassertv(false);
00409 }
00410 }
00411 }
00412
00413
00414 compose_with_order(mat, scale, shear, hpr, translate, get_order(), _coordsys);
00415 }
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432 bool EggXfmSAnim::
00433 set_value(int row, const LMatrix4d &mat) {
00434 nassertr(get_order() == get_standard_order(), false);
00435
00436 double components[num_matrix_components];
00437 bool add_ok = decompose_matrix(mat, components, _coordsys);
00438
00439
00440 #ifndef NDEBUG
00441 int table_length = -1;
00442 #endif
00443
00444 for (int i = 0; i < num_matrix_components; i++) {
00445 string name(1, matrix_component_letters[i]);
00446 EggNode *child = find_child(name);
00447 nassertr(child != (EggNode *)NULL &&
00448 child->is_of_type(EggSAnimData::get_class_type()), false);
00449 EggSAnimData *sanim = DCAST(EggSAnimData, child);
00450
00451 #ifndef NDEBUG
00452
00453 if (table_length < 0) {
00454 table_length = sanim->get_num_rows();
00455 } else {
00456 nassertr(sanim->get_num_rows() == table_length, false);
00457 }
00458 #endif
00459 sanim->set_value(row, components[i]);
00460 }
00461
00462 #ifndef NDEBUG
00463
00464 LMatrix4d new_mat;
00465 get_value(row, new_mat);
00466 if (!new_mat.almost_equal(mat, 0.005)) {
00467 egg_cat.warning()
00468 << "After set_row(" << row << ", ...) to:\n";
00469 mat.write(egg_cat.warning(false), 2);
00470 egg_cat.warning(false)
00471 << "which produces components:\n";
00472 for (int i = 0; i < num_matrix_components; i += 3) {
00473 egg_cat.warning(false)
00474 << " "
00475 << matrix_component_letters[i]
00476 << matrix_component_letters[i + 1]
00477 << matrix_component_letters[i + 2]
00478 << ": "
00479 << components[i] << " "
00480 << components[i + 1] << " "
00481 << components[i + 2] << "\n";
00482 }
00483 egg_cat.warning(false)
00484 << "new mat set was:\n";
00485 new_mat.write(egg_cat.warning(false), 2);
00486 return false;
00487 }
00488 #endif
00489
00490 return add_ok;
00491 }
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508
00509
00510
00511
00512
00513
00514
00515
00516
00517
00518
00519
00520
00521
00522
00523
00524 bool EggXfmSAnim::
00525 add_data(const LMatrix4d &mat) {
00526 double components[num_matrix_components];
00527 bool add_ok = decompose_matrix(mat, components, _coordsys);
00528
00529 if (empty()) {
00530
00531 for (int i = 0; i < num_matrix_components; i++) {
00532 char name = matrix_component_letters[i];
00533 EggSAnimData *sanim = new EggSAnimData(string(1, name));
00534 add_child(sanim);
00535 }
00536
00537
00538 set_order(get_standard_order());
00539 }
00540
00541 nassertr(get_order() == get_standard_order(), false);
00542
00543 #ifndef NDEBUG
00544 int table_length = -1;
00545 #endif
00546
00547 for (int i = 0; i < num_matrix_components; i++) {
00548 string name(1, matrix_component_letters[i]);
00549 EggNode *child = find_child(name);
00550 nassertr(child != (EggNode *)NULL &&
00551 child->is_of_type(EggSAnimData::get_class_type()), false);
00552 EggSAnimData *sanim = DCAST(EggSAnimData, child);
00553
00554 #ifndef NDEBUG
00555
00556 if (table_length < 0) {
00557 table_length = sanim->get_num_rows();
00558 } else {
00559 nassertr(sanim->get_num_rows() == table_length, false);
00560 }
00561 #endif
00562 sanim->add_data(components[i]);
00563 }
00564
00565 #ifndef NDEBUG
00566
00567 LMatrix4d new_mat;
00568 if (table_length >= 0) {
00569 get_value(table_length, new_mat);
00570 } else {
00571 get_value(0, new_mat);
00572 }
00573 if (!new_mat.almost_equal(mat, 0.005)) {
00574 egg_cat.warning()
00575 << "After add_data():\n";
00576 mat.write(egg_cat.warning(false), 2);
00577 egg_cat.warning(false)
00578 << "which produces components:\n";
00579 for (int i = 0; i < num_matrix_components; i += 3) {
00580 egg_cat.warning(false)
00581 << " "
00582 << matrix_component_letters[i]
00583 << matrix_component_letters[i + 1]
00584 << matrix_component_letters[i + 2]
00585 << ": "
00586 << components[i] << " "
00587 << components[i + 1] << " "
00588 << components[i + 2] << "\n";
00589 }
00590 egg_cat.warning(false)
00591 << "new mat set was:\n";
00592 new_mat.write(egg_cat.warning(false), 2);
00593 return false;
00594 }
00595 #endif
00596
00597 return add_ok;
00598 }
00599
00600
00601
00602
00603
00604
00605
00606 void EggXfmSAnim::
00607 add_component_data(const string &component_name, double value) {
00608 EggNode *child = find_child(component_name);
00609 EggSAnimData *sanim;
00610 if (child == (EggNode *)NULL) {
00611
00612 sanim = new EggSAnimData(component_name);
00613 add_child(sanim);
00614
00615 } else {
00616 DCAST_INTO_V(sanim, child);
00617 }
00618
00619 sanim->add_data(value);
00620 }
00621
00622
00623
00624
00625
00626
00627
00628 void EggXfmSAnim::
00629 add_component_data(int component, double value) {
00630 nassertv(component >= 0 && component < num_matrix_components);
00631
00632 string name(1, matrix_component_letters[component]);
00633 add_component_data(name, value);
00634 }
00635
00636
00637
00638
00639
00640
00641
00642
00643
00644 void EggXfmSAnim::
00645 r_transform(const LMatrix4d &mat, const LMatrix4d &inv,
00646 CoordinateSystem to_cs) {
00647
00648
00649 LMatrix4d inv1 = inv;
00650 inv1.set_row(3, LVector3d(0.0, 0.0, 0.0));
00651
00652
00653 EggXfmSAnim original;
00654 original.steal_children(*this);
00655 original = (*this);
00656
00657
00658 if (to_cs != CS_default) {
00659 _coordsys = to_cs;
00660 }
00661
00662 int num_rows = original.get_num_rows();
00663 LMatrix4d orig_mat;
00664 for (int r = 0; r < num_rows; r++) {
00665 original.get_value(r, orig_mat);
00666 bool result = add_data(inv1 * orig_mat * mat);
00667
00668
00669
00670
00671 nassertv(result);
00672 }
00673
00674
00675 optimize();
00676 }
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687 void EggXfmSAnim::
00688 r_mark_coordsys(CoordinateSystem cs) {
00689 _coordsys = cs;
00690 }
00691
00692
00693
00694
00695
00696
00697
00698
00699
00700
00701
00702 void EggXfmSAnim::
00703 normalize_by_rebuilding() {
00704
00705 EggXfmSAnim original;
00706 original.steal_children(*this);
00707 original = (*this);
00708
00709
00710 int num_rows = original.get_num_rows();
00711 LMatrix4d orig_mat;
00712 for (int r = 0; r < num_rows; r++) {
00713 original.get_value(r, orig_mat);
00714 bool result = add_data(orig_mat);
00715
00716
00717
00718
00719
00720 nassertv(result);
00721 }
00722 }
00723
00724
00725
00726
00727
00728
00729
00730
00731
00732
00733 void EggXfmSAnim::
00734 normalize_by_expanding() {
00735 iterator ci;
00736
00737
00738
00739 int num_tables = 0;
00740 int table_length = 1;
00741 string remaining_tables = matrix_component_letters;
00742
00743 for (ci = begin(); ci != end(); ++ci) {
00744 if ((*ci)->is_of_type(EggSAnimData::get_class_type())) {
00745 EggSAnimData *sanim = DCAST(EggSAnimData, *ci);
00746
00747 nassertv(sanim->get_name().length() == 1);
00748 char name = sanim->get_name()[0];
00749 size_t p = remaining_tables.find(name);
00750 nassertv(p != string::npos);
00751 remaining_tables[p] = ' ';
00752
00753 num_tables++;
00754 if (sanim->get_num_rows() > 1) {
00755 if (table_length == 1) {
00756 table_length = sanim->get_num_rows();
00757 } else {
00758 nassertv(sanim->get_num_rows() == table_length);
00759 }
00760 }
00761 }
00762 }
00763
00764 if (num_tables < num_matrix_components) {
00765
00766 for (size_t p = 0; p < remaining_tables.length(); p++) {
00767 if (remaining_tables[p] != ' ') {
00768 double default_value;
00769 switch (remaining_tables[p]) {
00770 case 'i':
00771 case 'j':
00772 case 'k':
00773 default_value = 1.0;
00774 break;
00775
00776 default:
00777 default_value = 0.0;
00778 }
00779
00780 string name(1, remaining_tables[p]);
00781 EggSAnimData *sanim = new EggSAnimData(name);
00782 add_child(sanim);
00783 sanim->add_data(default_value);
00784 }
00785 }
00786 }
00787
00788
00789 for (ci = begin(); ci != end(); ++ci) {
00790 if ((*ci)->is_of_type(EggSAnimData::get_class_type())) {
00791 EggSAnimData *sanim = DCAST(EggSAnimData, *ci);
00792 if (sanim->get_num_rows() == 1) {
00793 double value = sanim->get_value(0);
00794 for (int i = 1; i < table_length; i++) {
00795 sanim->add_data(value);
00796 }
00797 }
00798 nassertv(sanim->get_num_rows() == table_length);
00799 }
00800 }
00801 }