00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "pandabase.h"
00016 #include "hermiteCurve.h"
00017 #include "config_parametrics.h"
00018 #include "luse.h"
00019
00020 #include "indent.h"
00021 #include "datagram.h"
00022 #include "datagramIterator.h"
00023 #include "bamWriter.h"
00024 #include "bamReader.h"
00025
00026 #include <math.h>
00027
00028 TypeHandle HermiteCurve::_type_handle;
00029
00030 static const LVecBase3 zerovec_3 = LVecBase3(0.0f, 0.0f, 0.0f);
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 static ostream &
00041 show_vec3(ostream &out, int indent_level, const LVecBase3 &v,
00042 int num_dimensions) {
00043 indent(out, indent_level) << v[0];
00044 for (int i = 1; i<num_dimensions; i++) {
00045 out << " " << v[i];
00046 }
00047 return out;
00048 }
00049
00050
00051
00052
00053
00054
00055 HermiteCurveCV::
00056 HermiteCurveCV() {
00057 }
00058
00059
00060
00061
00062
00063
00064
00065 HermiteCurveCV::
00066 HermiteCurveCV(const HermiteCurveCV &c) :
00067 _p(c._p), _in(c._in), _out(c._out),
00068 _type(c._type), _name(c._name)
00069 {
00070 }
00071
00072
00073
00074
00075
00076
00077
00078 HermiteCurveCV::
00079 ~HermiteCurveCV() {
00080 }
00081
00082
00083
00084
00085
00086
00087
00088
00089 void HermiteCurveCV::
00090 set_in(const LVecBase3 &in) {
00091 _in = in;
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107 }
00108
00109
00110
00111
00112
00113
00114
00115 void HermiteCurveCV::
00116 set_out(const LVecBase3 &out) {
00117 _out = out;
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133 }
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148 void HermiteCurveCV::
00149 set_type(int type) {
00150 _type = type;
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162 }
00163
00164
00165
00166
00167
00168
00169
00170
00171 void HermiteCurveCV::
00172 set_name(const string &name) {
00173 _name = name;
00174 }
00175
00176
00177
00178
00179
00180
00181
00182 void HermiteCurveCV::
00183 format_egg(ostream &out, int indent_level, int num_dimensions,
00184 bool show_in, bool show_out,
00185 PN_stdfloat scale_in, PN_stdfloat scale_out) const {
00186 if (show_in) {
00187 indent(out, indent_level) << "<Vertex> {\n";
00188 show_vec3(out, indent_level + 2, _p - scale_in * _in / 3.0,
00189 num_dimensions) << "\n";
00190 indent(out, indent_level) << "}\n";
00191 }
00192
00193 indent(out, indent_level) << "<Vertex> {\n";
00194 show_vec3(out, indent_level + 2, _p, num_dimensions) << "\n";
00195
00196 indent(out, indent_level+2) << "<Scalar> continuity-type { ";
00197 switch (_type) {
00198 case HC_CUT:
00199 out << "Cut";
00200 break;
00201
00202 case HC_FREE:
00203 out << "Free";
00204 break;
00205
00206 case HC_G1:
00207 out << "G1";
00208 break;
00209
00210 case HC_SMOOTH:
00211 out << "Smooth";
00212 break;
00213 };
00214 out << " }\n";
00215
00216 indent(out, indent_level) << "}\n";
00217
00218 if (show_out) {
00219 indent(out, indent_level) << "<Vertex> {\n";
00220 show_vec3(out, indent_level + 2, _p + scale_out * _out / 3.0,
00221 num_dimensions) << "\n";
00222 indent(out, indent_level) << "}\n";
00223 }
00224 }
00225
00226
00227
00228
00229
00230
00231
00232 void HermiteCurveCV::
00233 write_datagram(BamWriter *, Datagram &me) const {
00234 _p.write_datagram(me);
00235 _in.write_datagram(me);
00236 _out.write_datagram(me);
00237 me.add_int8(_type);
00238 me.add_string(_name);
00239 }
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249 void HermiteCurveCV::
00250 fillin(DatagramIterator &scan, BamReader *) {
00251 _p.read_datagram(scan);
00252 _in.read_datagram(scan);
00253 _out.read_datagram(scan);
00254
00255 _type = scan.get_int8();
00256 _name = scan.get_string();
00257 }
00258
00259
00260
00261
00262
00263
00264 HermiteCurve::
00265 HermiteCurve() {
00266 }
00267
00268
00269
00270
00271
00272
00273
00274 HermiteCurve::
00275 HermiteCurve(const ParametricCurve &nc) {
00276 if (!nc.convert_to_hermite(this)) {
00277 parametrics_cat->warning()
00278 << "Cannot make a Hermite from the indicated curve."
00279 << endl;
00280 }
00281 }
00282
00283
00284
00285
00286
00287
00288
00289 HermiteCurve::
00290 ~HermiteCurve() {
00291 }
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303 int HermiteCurve::
00304 get_num_cvs() const {
00305 return _points.size();
00306 }
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325 int HermiteCurve::
00326 insert_cv(PN_stdfloat t) {
00327 if (!is_valid() || t >= get_max_t()) {
00328 int n = append_cv(HC_SMOOTH, 0.0f, 0.0f, 0.0f);
00329 set_cv_tstart(n, t);
00330 return n;
00331 }
00332
00333 t = min(max(t, (PN_stdfloat)0.0), get_max_t());
00334
00335 int n = find_cv(t);
00336 nassertr(n+1<get_num_cvs(), 0);
00337
00338 HermiteCurveCV cv;
00339 LVecBase3 tan;
00340 cv._type = HC_SMOOTH;
00341 get_pt(t, cv._p, tan);
00342 cv._out = cv._in = tan * 0.5f;
00343
00344 _points.insert(_points.begin() + n + 1, cv);
00345 bool result =
00346 insert_curveseg(n, new CubicCurveseg, t - get_cv_tstart(n));
00347 nassertr(result, 0);
00348
00349 recompute_basis();
00350 invalidate_all();
00351
00352 return n + 1;
00353 }
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363 int HermiteCurve::
00364 append_cv(int type, PN_stdfloat x, PN_stdfloat y, PN_stdfloat z) {
00365 HermiteCurveCV cv;
00366 cv.set_type(type);
00367 cv.set_point(LVecBase3(x, y, z));
00368 cv.set_in(zerovec_3);
00369 cv.set_out(zerovec_3);
00370 _points.push_back(cv);
00371 if (_points.size()>1) {
00372 bool result =
00373 insert_curveseg(_segs.size(), new CubicCurveseg, 1.0f);
00374 nassertr(result, 0);
00375 }
00376
00377 recompute_basis();
00378 invalidate_all();
00379
00380 return _points.size()-1;
00381 }
00382
00383
00384
00385
00386
00387
00388
00389
00390 bool HermiteCurve::
00391 remove_cv(int n) {
00392 if (n < 0 || n >= (int)_points.size()) {
00393 return false;
00394 }
00395
00396 _points.erase(_points.begin() + n);
00397 if (_segs.size()>0) {
00398 remove_curveseg(_segs.size()-1);
00399 }
00400
00401 recompute_basis();
00402 invalidate_all();
00403 return true;
00404 }
00405
00406
00407
00408
00409
00410
00411
00412 void HermiteCurve::
00413 remove_all_cvs() {
00414 _points.erase(_points.begin(), _points.end());
00415 remove_all_curvesegs();
00416
00417 invalidate_all();
00418 }
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428
00429
00430
00431
00432
00433
00434
00435
00436
00437
00438
00439
00440 bool HermiteCurve::
00441 set_cv_type(int n, int type) {
00442 if (n < 0 || n >= (int)_points.size()) {
00443 return false;
00444 }
00445
00446 bool changed_cut = false;
00447
00448 if (type!=_points[n]._type) {
00449 changed_cut = (type==HC_CUT || _points[n]._type==HC_CUT);
00450 _points[n].set_type(type);
00451 }
00452
00453 invalidate_cv(n, changed_cut);
00454 return true;
00455 }
00456
00457
00458
00459
00460
00461
00462
00463 bool HermiteCurve::
00464 set_cv_point(int n, PN_stdfloat x, PN_stdfloat y, PN_stdfloat z) {
00465 if (n < 0 || n >= (int)_points.size()) {
00466 return false;
00467 }
00468 _points[n].set_point(LVecBase3(x, y, z));
00469 invalidate_cv(n, false);
00470 return true;
00471 }
00472
00473
00474
00475
00476
00477
00478
00479
00480 bool HermiteCurve::
00481 set_cv_in(int n, PN_stdfloat x, PN_stdfloat y, PN_stdfloat z) {
00482 if (n < 0 || n >= (int)_points.size()) {
00483 return false;
00484 }
00485 _points[n].set_in(LVecBase3(x, y, z));
00486 invalidate_cv(n, false);
00487 return true;
00488 }
00489
00490
00491
00492
00493
00494
00495
00496
00497 bool HermiteCurve::
00498 set_cv_out(int n, PN_stdfloat x, PN_stdfloat y, PN_stdfloat z) {
00499 if (n < 0 || n >= (int)_points.size()) {
00500 return false;
00501 }
00502 _points[n].set_out(LVecBase3(x, y, z));
00503 invalidate_cv(n, false);
00504 return true;
00505 }
00506
00507
00508
00509
00510
00511
00512
00513 bool HermiteCurve::
00514 set_cv_tstart(int n, PN_stdfloat tstart) {
00515 if (n <= 0 || n >= (int)_points.size()) {
00516 return false;
00517 }
00518 if (fabs(tstart - get_cv_tstart(n)) > 0.0001f) {
00519 set_tlength(n-1, tstart - get_tstart(n-1));
00520 recompute_basis();
00521 invalidate_all();
00522 }
00523 return true;
00524 }
00525
00526
00527
00528
00529
00530
00531
00532 bool HermiteCurve::
00533 set_cv_name(int n, const char *name) {
00534 if (n < 0 || n >= (int)_points.size()) {
00535 return false;
00536 }
00537 _points[n].set_name(name);
00538 return true;
00539 }
00540
00541
00542
00543
00544
00545
00546
00547
00548
00549
00550 int HermiteCurve::
00551 get_cv_type(int n) const {
00552 if (n < 0 || n >= (int)_points.size()) {
00553 return 0;
00554 }
00555
00556 return _points[n]._type;
00557 }
00558
00559
00560
00561
00562
00563
00564
00565 const LVecBase3 &HermiteCurve::
00566 get_cv_point(int n) const {
00567 if (n < 0 || n >= (int)_points.size()) {
00568 return zerovec_3;
00569 }
00570
00571 return _points[n]._p;
00572 }
00573 void HermiteCurve::
00574 get_cv_point(int n, LVecBase3 &v) const {
00575 v = get_cv_point(n);
00576 }
00577
00578
00579
00580
00581
00582
00583
00584 const LVecBase3 &HermiteCurve::
00585 get_cv_in(int n) const {
00586 if (n < 0 || n >= (int)_points.size() || _points[n-1]._type==HC_CUT) {
00587 return zerovec_3;
00588 }
00589
00590 return _points[n]._in;
00591 }
00592 void HermiteCurve::
00593 get_cv_in(int n, LVecBase3 &v) const {
00594 v = get_cv_in(n);
00595 }
00596
00597
00598
00599
00600
00601
00602
00603 const LVecBase3 &HermiteCurve::
00604 get_cv_out(int n) const {
00605 if (n < 0 || n >= (int)_points.size() || _points[n]._type==HC_CUT) {
00606 return zerovec_3;
00607 }
00608
00609 return _points[n]._out;
00610 }
00611 void HermiteCurve::
00612 get_cv_out(int n, LVecBase3 &v) const {
00613 v = get_cv_out(n);
00614 }
00615
00616
00617
00618
00619
00620
00621
00622
00623 PN_stdfloat HermiteCurve::
00624 get_cv_tstart(int n) const {
00625 if (n<0) {
00626 return 0.0f;
00627 } else if (n >= (int)_points.size()) {
00628 return get_max_t();
00629 }
00630
00631 return get_tstart(n);
00632 }
00633
00634
00635
00636
00637
00638
00639 string HermiteCurve::
00640 get_cv_name(int n) const {
00641 if (n < 0 || n >= (int)_points.size()) {
00642 return string();
00643 }
00644
00645 return _points[n]._name;
00646 }
00647
00648
00649
00650
00651
00652
00653
00654 void HermiteCurve::
00655 output(ostream &out) const {
00656 PiecewiseCurve::output(out);
00657
00658 out << " (";
00659 switch (get_curve_type()) {
00660 case PCT_T:
00661 out << "in T, ";
00662 break;
00663
00664 case PCT_XYZ:
00665 out << "in XYZ, ";
00666 break;
00667
00668 case PCT_HPR:
00669 out << "in HPR, ";
00670 break;
00671
00672 default:
00673 break;
00674 }
00675
00676 out << get_num_cvs() << " CV's)";
00677 }
00678
00679
00680
00681
00682
00683
00684 void HermiteCurve::
00685 write_cv(ostream &out, int n) const {
00686 out << "CV";
00687 if (!get_cv_name(n).empty()) {
00688 out << " " << get_cv_name(n);
00689 }
00690
00691 out << " at t = " << get_cv_tstart(n)
00692 << "\npoint = " << get_cv_point(n)
00693 << "\nin = " << get_cv_in(n) << " out = " << get_cv_out(n)
00694 << "\ncontinuity type = ";
00695
00696 switch (get_cv_type(n)) {
00697 case HC_CUT:
00698 out << "Cut";
00699 break;
00700
00701 case HC_FREE:
00702 out << "Free";
00703 break;
00704
00705 case HC_G1:
00706 out << "G1";
00707 break;
00708
00709 case HC_SMOOTH:
00710 out << "Smooth";
00711 break;
00712
00713 default:
00714 break;
00715 }
00716
00717 out << "\n";
00718 }
00719
00720
00721
00722
00723
00724
00725
00726
00727
00728
00729
00730 bool HermiteCurve::
00731 rebuild_curveseg(int, PN_stdfloat, const LVecBase4 &,
00732 int, PN_stdfloat, const LVecBase4 &,
00733 int, PN_stdfloat, const LVecBase4 &,
00734 int, PN_stdfloat, const LVecBase4 &) {
00735 cerr << "rebuild_curveseg not implemented for this curve type.\n";
00736 return false;
00737 }
00738
00739
00740
00741
00742
00743
00744 bool HermiteCurve::
00745 format_egg(ostream &out, const string &name, const string &curve_type,
00746 int indent_level) const {
00747 indent(out, indent_level)
00748 << "<VertexPool> " << name << ".pool {\n";
00749
00750 int i;
00751 for (i = 0; i < (int)_points.size(); i++) {
00752 bool show_in = (i != 0);
00753 bool show_out = (i != (int)_points.size()-1);
00754 _points[i].format_egg(out, indent_level + 2, _num_dimensions,
00755 show_in, show_out,
00756 show_in ? get_tlength(i-1) : 0.0f,
00757 show_out ? get_tlength(i) : 0.0f);
00758 }
00759 indent(out, indent_level) << "}\n";
00760
00761 indent(out, indent_level) << "<BezierCurve> " << name << " {\n";
00762
00763 if (!curve_type.empty()) {
00764 indent(out, indent_level+2)
00765 << "<Scalar> type { " << curve_type << " }\n";
00766 }
00767
00768 indent(out, indent_level+2) << "<TLengths> {";
00769 if (_points.size() > 1) {
00770 for (i = 0; i < (int)_segs.size(); i++) {
00771 if (i%10 == 1) {
00772 out << "\n";
00773 indent(out, indent_level+3);
00774 }
00775 out << " " << get_tlength(i);
00776 }
00777 }
00778 out << "\n";
00779 indent(out, indent_level+2) << "}\n";
00780
00781 indent(out, indent_level+2) << "<VertexRef> {";
00782 for (i = 1; i <= (int)_points.size() * 3 - 2; i++) {
00783 if (i%10 == 1) {
00784 out << "\n";
00785 indent(out, indent_level+3);
00786 }
00787 out << " " << i;
00788 }
00789 out << "\n";
00790 indent(out, indent_level+4) << "<Ref> { " << name << ".pool }\n";
00791 indent(out, indent_level+2) << "}\n";
00792
00793 indent(out, indent_level) << "}\n";
00794
00795 return true;
00796 }
00797
00798
00799 static void
00800 wrap_hpr(const LVecBase3 &hpr1, LVecBase3 &hpr2) {
00801 for (int i = 0; i < 3; i++) {
00802 while ((hpr2[i] - hpr1[i]) > 180.0) {
00803 hpr2[i] -= 360.0;
00804 }
00805
00806 while ((hpr2[i] - hpr1[i]) < -180.0) {
00807 hpr2[i] += 360.0;
00808 }
00809 }
00810 }
00811
00812
00813
00814
00815
00816
00817
00818 void HermiteCurve::
00819 invalidate_cv(int n, bool redo_all) {
00820 PN_stdfloat t1 = 0.0f, t2 = get_max_t();
00821 if (n>0 && _points[n-1]._type!=HC_CUT) {
00822 const HermiteCurveCV &p1 = _points[n-1];
00823 HermiteCurveCV p2(_points[n]);
00824 if (_curve_type == PCT_HPR) {
00825 wrap_hpr(p1._p, p2._p);
00826 }
00827 get_curveseg(n-1)->hermite_basis(p1, p2, get_tlength(n-1));
00828 t1 = get_cv_tstart(n-1);
00829 }
00830
00831 if (n+1 < (int)_points.size()) {
00832 if (_points[n]._type==HC_CUT) {
00833 BezierSeg seg;
00834 seg._v[0] = seg._v[1] = seg._v[2] = seg._v[3] = _points[n]._p;
00835 get_curveseg(n)->bezier_basis(seg);
00836 } else {
00837 const HermiteCurveCV &p1 = _points[n];
00838 HermiteCurveCV p2(_points[n+1]);
00839 if (_curve_type == PCT_HPR) {
00840 wrap_hpr(p1._p, p2._p);
00841 }
00842 get_curveseg(n)->hermite_basis(p1, p2, get_tlength(n));
00843 t2 = get_cv_tstart(n+2);
00844 }
00845 }
00846
00847 if (is_valid()) {
00848 if (redo_all) {
00849 invalidate_all();
00850 } else {
00851 invalidate(t1, t2);
00852 }
00853 }
00854 }
00855
00856
00857
00858
00859
00860
00861
00862
00863
00864 int HermiteCurve::
00865 find_cv(PN_stdfloat t) {
00866 nassertr(is_valid(), 0);
00867
00868 int n;
00869 for (n = 0; n < (int)_segs.size(); n++) {
00870 if (_segs[n]._tend+0.00001 > t) {
00871 break;
00872 }
00873 }
00874
00875 return n;
00876 }
00877
00878
00879
00880
00881
00882
00883
00884
00885
00886
00887 void HermiteCurve::
00888 recompute_basis() {
00889 int n;
00890 for (n = 0; n < (int)_segs.size(); n++) {
00891 if (_points[n]._type==HC_CUT) {
00892 BezierSeg seg;
00893 seg._v[0] = seg._v[1] = seg._v[2] = seg._v[3] = _points[n]._p;
00894 get_curveseg(n)->bezier_basis(seg);
00895 } else {
00896 const HermiteCurveCV &p1 = _points[n];
00897 HermiteCurveCV p2(_points[n+1]);
00898 if (_curve_type == PCT_HPR) {
00899 wrap_hpr(p1._p, p2._p);
00900 }
00901 get_curveseg(n)->hermite_basis(p1, p2, get_tlength(n));
00902 }
00903 }
00904 }
00905
00906
00907
00908
00909
00910
00911
00912
00913 void HermiteCurve::
00914 register_with_read_factory() {
00915 BamReader::get_factory()->register_factory(get_class_type(), make_HermiteCurve);
00916 }
00917
00918
00919
00920
00921
00922
00923 TypedWritable *HermiteCurve::
00924 make_HermiteCurve(const FactoryParams ¶ms) {
00925 HermiteCurve *me = new HermiteCurve;
00926 DatagramIterator scan;
00927 BamReader *manager;
00928
00929 parse_params(params, scan, manager);
00930 me->fillin(scan, manager);
00931 return me;
00932 }
00933
00934
00935
00936
00937
00938
00939
00940 void HermiteCurve::
00941 write_datagram(BamWriter *manager, Datagram &me) {
00942 PiecewiseCurve::write_datagram(manager, me);
00943
00944 me.add_uint32(_points.size());
00945 size_t i;
00946 for (i = 0; i < _points.size(); i++) {
00947 _points[i].write_datagram(manager, me);
00948 }
00949 }
00950
00951
00952
00953
00954
00955
00956
00957
00958
00959 void HermiteCurve::
00960 fillin(DatagramIterator &scan, BamReader *manager) {
00961 PiecewiseCurve::fillin(scan, manager);
00962
00963 size_t num_points = scan.get_uint32();
00964 _points.reserve(num_points);
00965 size_t i;
00966 for (i = 0; i < num_points; i++) {
00967 HermiteCurveCV cv;
00968 cv.fillin(scan, manager);
00969 _points.push_back(cv);
00970 }
00971 }