00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "eggJointData.h"
00016 #include "eggJointNodePointer.h"
00017 #include "eggMatrixTablePointer.h"
00018 #include "pvector.h"
00019 #include "dcast.h"
00020 #include "eggGroup.h"
00021 #include "eggTable.h"
00022 #include "indent.h"
00023 #include "fftCompressor.h"
00024 #include "zStream.h"
00025
00026 TypeHandle EggJointData::_type_handle;
00027
00028
00029
00030
00031
00032
00033
00034 EggJointData::
00035 EggJointData(EggCharacterCollection *collection,
00036 EggCharacterData *char_data) :
00037 EggComponentData(collection, char_data)
00038 {
00039 _parent = (EggJointData *)NULL;
00040 _new_parent = (EggJointData *)NULL;
00041 _has_rest_frame = false;
00042 _rest_frames_differ = false;
00043 }
00044
00045
00046
00047
00048
00049
00050
00051
00052 LMatrix4d EggJointData::
00053 get_frame(int model_index, int n) const {
00054 EggBackPointer *back = get_model(model_index);
00055 if (back == (EggBackPointer *)NULL) {
00056 return LMatrix4d::ident_mat();
00057 }
00058
00059 EggJointPointer *joint;
00060 DCAST_INTO_R(joint, back, LMatrix4d::ident_mat());
00061
00062 return joint->get_frame(n);
00063 }
00064
00065
00066
00067
00068
00069
00070
00071
00072 LMatrix4d EggJointData::
00073 get_net_frame(int model_index, int n, EggCharacterDb &db) const {
00074 EggBackPointer *back = get_model(model_index);
00075 if (back == (EggBackPointer *)NULL) {
00076 return LMatrix4d::ident_mat();
00077 }
00078
00079 EggJointPointer *joint;
00080 DCAST_INTO_R(joint, back, LMatrix4d::ident_mat());
00081
00082 LMatrix4d mat;
00083 if (!db.get_matrix(joint, EggCharacterDb::TT_net_frame, n, mat)) {
00084
00085 mat = get_frame(model_index, n);
00086 if (_parent != (EggJointData *)NULL) {
00087 mat = mat * _parent->get_net_frame(model_index, n, db);
00088 }
00089 db.set_matrix(joint, EggCharacterDb::TT_net_frame, n, mat);
00090 }
00091
00092 return mat;
00093 }
00094
00095
00096
00097
00098
00099
00100 LMatrix4d EggJointData::
00101 get_net_frame_inv(int model_index, int n, EggCharacterDb &db) const {
00102 EggBackPointer *back = get_model(model_index);
00103 if (back == (EggBackPointer *)NULL) {
00104 return LMatrix4d::ident_mat();
00105 }
00106
00107 EggJointPointer *joint;
00108 DCAST_INTO_R(joint, back, LMatrix4d::ident_mat());
00109
00110 LMatrix4d mat;
00111 if (!db.get_matrix(joint, EggCharacterDb::TT_net_frame_inv, n, mat)) {
00112
00113 LMatrix4d mat = get_net_frame(model_index, n, db);
00114 mat.invert_in_place();
00115 db.set_matrix(joint, EggCharacterDb::TT_net_frame_inv, n, mat);
00116 }
00117
00118 return mat;
00119 }
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130 void EggJointData::
00131 force_initial_rest_frame() {
00132 if (!has_rest_frame()) {
00133 return;
00134 }
00135 int num_models = get_num_models();
00136 for (int model_index = 0; model_index < num_models; model_index++) {
00137 if (has_model(model_index)) {
00138 EggJointPointer *joint;
00139 DCAST_INTO_V(joint, get_model(model_index));
00140 if (joint->is_of_type(EggJointNodePointer::get_class_type())) {
00141 joint->set_frame(0, get_rest_frame());
00142 }
00143 }
00144 }
00145 _rest_frames_differ = false;
00146 }
00147
00148
00149
00150
00151
00152
00153
00154
00155 void EggJointData::
00156 move_vertices_to(EggJointData *new_owner) {
00157 int num_models = get_num_models();
00158
00159 if (new_owner == (EggJointData *)NULL) {
00160 for (int model_index = 0; model_index < num_models; model_index++) {
00161 if (has_model(model_index)) {
00162 EggJointPointer *joint;
00163 DCAST_INTO_V(joint, get_model(model_index));
00164 joint->move_vertices_to((EggJointPointer *)NULL);
00165 }
00166 }
00167 } else {
00168 for (int model_index = 0; model_index < num_models; model_index++) {
00169 if (has_model(model_index) && new_owner->has_model(model_index)) {
00170 EggJointPointer *joint, *new_joint;
00171 DCAST_INTO_V(joint, get_model(model_index));
00172 DCAST_INTO_V(new_joint, new_owner->get_model(model_index));
00173 joint->move_vertices_to(new_joint);
00174 }
00175 }
00176 }
00177 }
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190 int EggJointData::
00191 score_reparent_to(EggJointData *new_parent, EggCharacterDb &db) {
00192 if (!FFTCompressor::is_compression_available()) {
00193
00194
00195 return -1;
00196 }
00197
00198
00199
00200
00201 vector_stdfloat i, j, k, a, b, c, x, y, z;
00202 pvector<LVecBase3> hprs;
00203 int num_rows = 0;
00204
00205 int num_models = get_num_models();
00206 for (int model_index = 0; model_index < num_models; model_index++) {
00207 EggBackPointer *back = get_model(model_index);
00208 if (back != (EggBackPointer *)NULL) {
00209 EggJointPointer *joint;
00210 DCAST_INTO_R(joint, back, false);
00211
00212 int num_frames = get_num_frames(model_index);
00213 for (int n = 0; n < num_frames; n++) {
00214 LMatrix4d transform;
00215 if (_parent == new_parent) {
00216
00217 transform = LMatrix4d::ident_mat();
00218
00219 } else if (_parent == (EggJointData *)NULL) {
00220
00221 transform = new_parent->get_net_frame_inv(model_index, n, db);
00222
00223 } else if (new_parent == (EggJointData *)NULL) {
00224
00225 transform = _parent->get_net_frame(model_index, n, db);
00226
00227 } else {
00228
00229 transform =
00230 _parent->get_net_frame(model_index, n, db) *
00231 new_parent->get_net_frame_inv(model_index, n, db);
00232 }
00233
00234 transform = joint->get_frame(n) * transform;
00235 LVecBase3d scale, shear, hpr, translate;
00236 if (!decompose_matrix(transform, scale, shear, hpr, translate)) {
00237
00238 return -1;
00239 }
00240 i.push_back(scale[0]);
00241 j.push_back(scale[1]);
00242 k.push_back(scale[2]);
00243 a.push_back(shear[0]);
00244 b.push_back(shear[1]);
00245 c.push_back(shear[2]);
00246 hprs.push_back(LCAST(PN_stdfloat, hpr));
00247 x.push_back(translate[0]);
00248 y.push_back(translate[1]);
00249 z.push_back(translate[2]);
00250 num_rows++;
00251 }
00252 }
00253 }
00254
00255 if (num_rows == 0) {
00256
00257 return -1;
00258 }
00259
00260
00261
00262
00263 FFTCompressor compressor;
00264 Datagram dg;
00265 compressor.write_reals(dg, &i[0], num_rows);
00266 compressor.write_reals(dg, &j[0], num_rows);
00267 compressor.write_reals(dg, &k[0], num_rows);
00268 compressor.write_reals(dg, &a[0], num_rows);
00269 compressor.write_reals(dg, &b[0], num_rows);
00270 compressor.write_reals(dg, &c[0], num_rows);
00271 compressor.write_hprs(dg, &hprs[0], num_rows);
00272 compressor.write_reals(dg, &x[0], num_rows);
00273 compressor.write_reals(dg, &y[0], num_rows);
00274 compressor.write_reals(dg, &z[0], num_rows);
00275
00276
00277 #ifndef HAVE_ZLIB
00278 return dg.get_length();
00279
00280 #else
00281
00282
00283
00284 ostringstream sstr;
00285 OCompressStream zstr(&sstr, false);
00286 zstr.write((const char *)dg.get_data(), dg.get_length());
00287 zstr.flush();
00288 return sstr.str().length();
00289 #endif
00290 }
00291
00292
00293
00294
00295
00296
00297
00298
00299 bool EggJointData::
00300 do_rebuild_all(EggCharacterDb &db) {
00301 bool all_ok = true;
00302
00303 BackPointers::iterator bpi;
00304 for (bpi = _back_pointers.begin(); bpi != _back_pointers.end(); ++bpi) {
00305 EggBackPointer *back = (*bpi);
00306 if (back != (EggBackPointer *)NULL) {
00307 EggJointPointer *joint;
00308 DCAST_INTO_R(joint, back, false);
00309 if (!joint->do_rebuild(db)) {
00310 all_ok = false;
00311 }
00312 }
00313 }
00314
00315 Children::iterator ci;
00316 for (ci = _children.begin(); ci != _children.end(); ++ci) {
00317 EggJointData *child = (*ci);
00318 if (!child->do_rebuild_all(db)) {
00319 all_ok = false;
00320 }
00321 }
00322
00323 return all_ok;
00324 }
00325
00326
00327
00328
00329
00330
00331
00332 void EggJointData::
00333 optimize() {
00334 BackPointers::iterator bpi;
00335 for (bpi = _back_pointers.begin(); bpi != _back_pointers.end(); ++bpi) {
00336 EggBackPointer *back = (*bpi);
00337 if (back != (EggBackPointer *)NULL) {
00338 EggJointPointer *joint;
00339 DCAST_INTO_V(joint, back);
00340 joint->optimize();
00341 }
00342 }
00343
00344 Children::iterator ci;
00345 for (ci = _children.begin(); ci != _children.end(); ++ci) {
00346 EggJointData *child = (*ci);
00347 child->optimize();
00348 }
00349 }
00350
00351
00352
00353
00354
00355
00356
00357 void EggJointData::
00358 expose(EggGroup::DCSType dcs_type) {
00359 BackPointers::iterator bpi;
00360 for (bpi = _back_pointers.begin(); bpi != _back_pointers.end(); ++bpi) {
00361 EggBackPointer *back = (*bpi);
00362 if (back != (EggBackPointer *)NULL) {
00363 EggJointPointer *joint;
00364 DCAST_INTO_V(joint, back);
00365 joint->expose(dcs_type);
00366 }
00367 }
00368 }
00369
00370
00371
00372
00373
00374
00375
00376 void EggJointData::
00377 zero_channels(const string &components) {
00378 BackPointers::iterator bpi;
00379 for (bpi = _back_pointers.begin(); bpi != _back_pointers.end(); ++bpi) {
00380 EggBackPointer *back = (*bpi);
00381 if (back != (EggBackPointer *)NULL) {
00382 EggJointPointer *joint;
00383 DCAST_INTO_V(joint, back);
00384 joint->zero_channels(components);
00385 }
00386 }
00387 }
00388
00389
00390
00391
00392
00393
00394
00395 void EggJointData::
00396 quantize_channels(const string &components, double quantum) {
00397 BackPointers::iterator bpi;
00398 for (bpi = _back_pointers.begin(); bpi != _back_pointers.end(); ++bpi) {
00399 EggBackPointer *back = (*bpi);
00400 if (back != (EggBackPointer *)NULL) {
00401 EggJointPointer *joint;
00402 DCAST_INTO_V(joint, back);
00403 joint->quantize_channels(components, quantum);
00404 }
00405 }
00406
00407 Children::iterator ci;
00408 for (ci = _children.begin(); ci != _children.end(); ++ci) {
00409 EggJointData *child = (*ci);
00410 child->quantize_channels(components, quantum);
00411 }
00412 }
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422 void EggJointData::
00423 apply_default_pose(int source_model, int frame) {
00424 if (has_model(source_model)) {
00425 EggJointPointer *source_joint;
00426 DCAST_INTO_V(source_joint, _back_pointers[source_model]);
00427 BackPointers::iterator bpi;
00428 for (bpi = _back_pointers.begin(); bpi != _back_pointers.end(); ++bpi) {
00429 EggBackPointer *back = (*bpi);
00430 if (back != (EggBackPointer *)NULL) {
00431 EggJointPointer *joint;
00432 DCAST_INTO_V(joint, back);
00433 joint->apply_default_pose(source_joint, frame);
00434 }
00435 }
00436 }
00437
00438 Children::iterator ci;
00439 for (ci = _children.begin(); ci != _children.end(); ++ci) {
00440 EggJointData *child = (*ci);
00441 child->apply_default_pose(source_model, frame);
00442 }
00443 }
00444
00445
00446
00447
00448
00449
00450
00451 void EggJointData::
00452 add_back_pointer(int model_index, EggObject *egg_object) {
00453 nassertv(egg_object != (EggObject *)NULL);
00454 if (egg_object->is_of_type(EggGroup::get_class_type())) {
00455
00456 EggJointNodePointer *joint = new EggJointNodePointer(egg_object);
00457 set_model(model_index, joint);
00458 if (!_has_rest_frame) {
00459 _rest_frame = joint->get_frame(0);
00460 _has_rest_frame = true;
00461
00462 } else {
00463
00464
00465
00466 if (!_rest_frame.almost_equal(joint->get_frame(0), 0.0001)) {
00467 _rest_frames_differ = true;
00468 }
00469 }
00470
00471 } else if (egg_object->is_of_type(EggTable::get_class_type())) {
00472
00473 EggMatrixTablePointer *xform = new EggMatrixTablePointer(egg_object);
00474 set_model(model_index, xform);
00475
00476 } else {
00477 nout << "Invalid object added to joint for back pointer.\n";
00478 }
00479 }
00480
00481
00482
00483
00484
00485
00486 void EggJointData::
00487 write(ostream &out, int indent_level) const {
00488 indent(out, indent_level)
00489 << "Joint " << get_name()
00490 << " (models:";
00491 int num_models = get_num_models();
00492 for (int model_index = 0; model_index < num_models; model_index++) {
00493 if (has_model(model_index)) {
00494 out << " " << model_index;
00495 }
00496 }
00497 out << ") {\n";
00498
00499 Children::const_iterator ci;
00500 for (ci = _children.begin(); ci != _children.end(); ++ci) {
00501 (*ci)->write(out, indent_level + 2);
00502 }
00503
00504 indent(out, indent_level) << "}\n";
00505 }
00506
00507
00508
00509
00510
00511
00512
00513 void EggJointData::
00514 do_begin_reparent() {
00515 _got_new_parent_depth = false;
00516 _children.clear();
00517 }
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530
00531
00532
00533
00534
00535 bool EggJointData::
00536 calc_new_parent_depth(pset<EggJointData *> &chain) {
00537 if (_got_new_parent_depth) {
00538 return false;
00539 }
00540 if (_new_parent == (EggJointData *)NULL) {
00541
00542 _got_new_parent_depth = true;
00543 _new_parent_depth = 0;
00544 return false;
00545 }
00546 if (!chain.insert(this).second) {
00547
00548 return true;
00549 }
00550 bool cycle = _new_parent->calc_new_parent_depth(chain);
00551 _new_parent_depth = _new_parent->_new_parent_depth + 1;
00552 return cycle;
00553 }
00554
00555
00556
00557
00558
00559
00560
00561
00562 void EggJointData::
00563 do_begin_compute_reparent() {
00564 _got_new_net_frame = false;
00565 _got_new_net_frame_inv = false;
00566 _computed_reparent = false;
00567 }
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00578 bool EggJointData::
00579 do_compute_reparent(int model_index, int n, EggCharacterDb &db) {
00580 if (_computed_reparent) {
00581
00582
00583
00584 return _computed_ok;
00585 }
00586 _computed_reparent = true;
00587
00588 if (_parent == _new_parent) {
00589
00590
00591 _computed_ok = true;
00592 return true;
00593 }
00594
00595 EggBackPointer *back = get_model(model_index);
00596 if (back == (EggBackPointer *)NULL) {
00597
00598 _computed_ok = true;
00599 return true;
00600 }
00601
00602 EggJointPointer *joint;
00603 DCAST_INTO_R(joint, back, false);
00604
00605 LMatrix4d transform;
00606 if (_parent == (EggJointData *)NULL) {
00607
00608 transform = _new_parent->get_new_net_frame_inv(model_index, n, db);
00609
00610 } else if (_new_parent == (EggJointData *)NULL) {
00611
00612 transform = _parent->get_net_frame(model_index, n, db);
00613
00614 } else {
00615
00616 transform =
00617 _parent->get_net_frame(model_index, n, db) *
00618 _new_parent->get_new_net_frame_inv(model_index, n, db);
00619 }
00620
00621 db.set_matrix(joint, EggCharacterDb::TT_rebuild_frame, n,
00622 joint->get_frame(n) * transform);
00623 _computed_ok = true;
00624
00625 return _computed_ok;
00626 }
00627
00628
00629
00630
00631
00632
00633
00634
00635 bool EggJointData::
00636 do_joint_rebuild(int model_index, EggCharacterDb &db) {
00637 bool all_ok = true;
00638
00639 EggJointPointer *parent_joint = NULL;
00640 if (_new_parent != NULL && _new_parent->has_model(model_index)) {
00641 DCAST_INTO_R(parent_joint, _new_parent->get_model(model_index), false);
00642 }
00643
00644 if (has_model(model_index)) {
00645 EggJointPointer *joint;
00646 DCAST_INTO_R(joint, get_model(model_index), false);
00647 if (!joint->do_rebuild(db)) {
00648 all_ok = false;
00649 }
00650 }
00651
00652 return all_ok;
00653 }
00654
00655
00656
00657
00658
00659
00660
00661
00662 void EggJointData::
00663 do_finish_reparent() {
00664 int num_models = get_num_models();
00665 for (int model_index = 0; model_index < num_models; model_index++) {
00666 EggJointPointer *parent_joint = NULL;
00667 if (_new_parent != NULL && _new_parent->has_model(model_index)) {
00668 DCAST_INTO_V(parent_joint, _new_parent->get_model(model_index));
00669 }
00670
00671 if (has_model(model_index)) {
00672 EggJointPointer *joint;
00673 DCAST_INTO_V(joint, get_model(model_index));
00674 joint->do_finish_reparent(parent_joint);
00675 }
00676 }
00677
00678 _parent = _new_parent;
00679 if (_parent != (EggJointData *)NULL) {
00680 _parent->_children.push_back(this);
00681 }
00682 }
00683
00684
00685
00686
00687
00688
00689
00690
00691 EggJointData *EggJointData::
00692 make_new_joint(const string &name) {
00693 EggJointData *child = new EggJointData(_collection, _char_data);
00694 child->set_name(name);
00695 child->_parent = this;
00696 child->_new_parent = this;
00697 _children.push_back(child);
00698
00699
00700 int num_models = get_num_models();
00701 for (int i = 0; i < num_models; i++) {
00702 if (has_model(i)) {
00703 EggJointPointer *joint;
00704 DCAST_INTO_R(joint, get_model(i), NULL);
00705 EggJointPointer *new_joint = joint->make_new_joint(name);
00706 child->set_model(i, new_joint);
00707 }
00708 }
00709
00710 return child;
00711 }
00712
00713
00714
00715
00716
00717
00718
00719
00720 EggJointData *EggJointData::
00721 find_joint_exact(const string &name) {
00722 Children::const_iterator ci;
00723 for (ci = _children.begin(); ci != _children.end(); ++ci) {
00724 EggJointData *child = (*ci);
00725 if (child->get_name() == name) {
00726 return child;
00727 }
00728 EggJointData *result = child->find_joint_exact(name);
00729 if (result != (EggJointData *)NULL) {
00730 return result;
00731 }
00732 }
00733
00734 return (EggJointData *)NULL;
00735 }
00736
00737
00738
00739
00740
00741
00742
00743 EggJointData *EggJointData::
00744 find_joint_matches(const string &name) {
00745 Children::const_iterator ci;
00746 for (ci = _children.begin(); ci != _children.end(); ++ci) {
00747 EggJointData *child = (*ci);
00748 if (child->matches_name(name)) {
00749 return child;
00750 }
00751 EggJointData *result = child->find_joint_matches(name);
00752 if (result != (EggJointData *)NULL) {
00753 return result;
00754 }
00755 }
00756
00757 return (EggJointData *)NULL;
00758 }
00759
00760
00761
00762
00763
00764
00765
00766
00767
00768 bool EggJointData::
00769 is_new_ancestor(EggJointData *child) const {
00770 if (child == this) {
00771 return true;
00772 }
00773
00774 if (child->_new_parent == (EggJointData *)NULL) {
00775 return false;
00776 }
00777
00778 return is_new_ancestor(child->_new_parent);
00779 }
00780
00781
00782
00783
00784
00785
00786
00787
00788
00789 const LMatrix4d &EggJointData::
00790 get_new_net_frame(int model_index, int n, EggCharacterDb &db) {
00791 if (!_got_new_net_frame) {
00792 _new_net_frame = get_new_frame(model_index, n, db);
00793 if (_new_parent != (EggJointData *)NULL) {
00794 _new_net_frame = _new_net_frame * _new_parent->get_new_net_frame(model_index, n, db);
00795 }
00796 _got_new_net_frame = true;
00797 }
00798 return _new_net_frame;
00799 }
00800
00801
00802
00803
00804
00805
00806 const LMatrix4d &EggJointData::
00807 get_new_net_frame_inv(int model_index, int n, EggCharacterDb &db) {
00808 if (!_got_new_net_frame_inv) {
00809 _new_net_frame_inv.invert_from(get_new_frame(model_index, n, db));
00810 if (_new_parent != (EggJointData *)NULL) {
00811 _new_net_frame_inv = _new_parent->get_new_net_frame_inv(model_index, n, db) * _new_net_frame_inv;
00812 }
00813 _got_new_net_frame_inv = true;
00814 }
00815 return _new_net_frame_inv;
00816 }
00817
00818
00819
00820
00821
00822
00823
00824
00825
00826 LMatrix4d EggJointData::
00827 get_new_frame(int model_index, int n, EggCharacterDb &db) {
00828 do_compute_reparent(model_index, n, db);
00829
00830 EggBackPointer *back = get_model(model_index);
00831 if (back == (EggBackPointer *)NULL) {
00832 return LMatrix4d::ident_mat();
00833 }
00834
00835 EggJointPointer *joint;
00836 DCAST_INTO_R(joint, back, LMatrix4d::ident_mat());
00837
00838 LMatrix4d mat;
00839 if (!db.get_matrix(joint, EggCharacterDb::TT_rebuild_frame, n, mat)) {
00840
00841 return joint->get_frame(n);
00842 }
00843
00844
00845 return mat;
00846 }