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