00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015 #include "pandabase.h"
00016 #include "pointerTo.h"
00017
00018 #include "curveFitter.h"
00019 #include "config_parametrics.h"
00020 #include "parametricCurve.h"
00021 #include "nurbsCurve.h"
00022 #include "hermiteCurve.h"
00023 #include <algorithm>
00024
00025 TypeHandle CurveFitter::_type_handle;
00026
00027
00028
00029
00030
00031
00032 CurveFitter::
00033 CurveFitter() {
00034 _got_xyz = false;
00035 _got_hpr = false;
00036 }
00037
00038
00039
00040
00041
00042
00043 CurveFitter::
00044 ~CurveFitter() {
00045 }
00046
00047
00048
00049
00050
00051
00052
00053 void CurveFitter::
00054 reset() {
00055 _data.erase(_data.begin(), _data.end());
00056 }
00057
00058
00059
00060
00061
00062
00063 void CurveFitter::
00064 add_xyz(PN_stdfloat t, const LVecBase3 &xyz) {
00065 DataPoint dp;
00066 dp._t = t;
00067 dp._xyz = xyz;
00068 _data.push_back(dp);
00069 _got_xyz = true;
00070 }
00071
00072
00073
00074
00075
00076
00077 void CurveFitter::
00078 add_hpr(PN_stdfloat t, const LVecBase3 &hpr) {
00079 DataPoint dp;
00080 dp._t = t;
00081 dp._hpr = hpr;
00082 _data.push_back(dp);
00083 _got_hpr = true;
00084 }
00085
00086
00087
00088
00089
00090
00091 void CurveFitter::
00092 add_xyz_hpr(PN_stdfloat t, const LVecBase3 &xyz, const LVecBase3 &hpr) {
00093 DataPoint dp;
00094 dp._t = t;
00095 dp._xyz = xyz;
00096 dp._hpr = hpr;
00097 _data.push_back(dp);
00098 _got_xyz = true;
00099 _got_hpr = true;
00100 }
00101
00102
00103
00104
00105
00106
00107
00108 int CurveFitter::
00109 get_num_samples() const {
00110 return _data.size();
00111 }
00112
00113
00114
00115
00116
00117
00118 PN_stdfloat CurveFitter::
00119 get_sample_t(int n) const {
00120 nassertr(n >= 0 && n < (int)_data.size(), 0.0f);
00121 return _data[n]._t;
00122 }
00123
00124
00125
00126
00127
00128
00129 LVecBase3 CurveFitter::
00130 get_sample_xyz(int n) const {
00131 nassertr(n >= 0 && n < (int)_data.size(), LVecBase3::zero());
00132 return _data[n]._xyz;
00133 }
00134
00135
00136
00137
00138
00139
00140 LVecBase3 CurveFitter::
00141 get_sample_hpr(int n) const {
00142 nassertr(n >= 0 && n < (int)_data.size(), LVecBase3::zero());
00143 return _data[n]._hpr;
00144 }
00145
00146
00147
00148
00149
00150
00151
00152
00153 LVecBase3 CurveFitter::
00154 get_sample_tangent(int n) const {
00155 nassertr(n >= 0 && n < (int)_data.size(), LVecBase3::zero());
00156 return _data[n]._tangent;
00157 }
00158
00159
00160
00161
00162
00163
00164
00165 void CurveFitter::
00166 remove_samples(int begin, int end) {
00167 begin = max(0, min((int)_data.size(), begin));
00168 end = max(0, min((int)_data.size(), end));
00169
00170 nassertv(begin <= end);
00171
00172 _data.erase(_data.begin() + begin, _data.begin() + end);
00173 }
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183 void CurveFitter::
00184 sample(ParametricCurveCollection *curves, int count) {
00185 nassertv(curves != (ParametricCurveCollection *)NULL);
00186 PN_stdfloat max_t = curves->get_max_t();
00187 PN_stdfloat t, last_t, d;
00188 DataPoint dp;
00189
00190 last_t = 0.0f;
00191 d = 0.0f;
00192 int i;
00193 for (i = 0; i < count; i++) {
00194 t = max_t * (PN_stdfloat)i / (PN_stdfloat)(count-1);
00195 if (curves->evaluate(t, dp._xyz, dp._hpr)) {
00196 dp._t = t;
00197 _data.push_back(dp);
00198 }
00199 }
00200
00201 if (curves->get_xyz_curve() != (ParametricCurve *)NULL) {
00202 _got_xyz = true;
00203 }
00204 if (curves->get_hpr_curve() != (ParametricCurve *)NULL) {
00205 _got_hpr = true;
00206 }
00207 }
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218 void CurveFitter::
00219 wrap_hpr() {
00220 Data::iterator di;
00221 LVecBase3 last(0.0f, 0.0f, 0.0f);
00222 LVecBase3 net(0.0f, 0.0f, 0.0f);
00223
00224 for (di = _data.begin(); di != _data.end(); ++di) {
00225 int i;
00226 for (i = 0; i < 3; i++) {
00227 (*di)._hpr[i] += net[i];
00228
00229 while (((*di)._hpr[i] - last[i]) > 180.0f) {
00230 (*di)._hpr[i] -= 360.0f;
00231 net[i] -= 360.0f;
00232 }
00233
00234 while (((*di)._hpr[i] - last[i]) < -180.0f) {
00235 (*di)._hpr[i] += 360.0f;
00236 net[i] += 360.0f;
00237 }
00238
00239 last[i] = (*di)._hpr[i];
00240 }
00241 }
00242 }
00243
00244
00245
00246
00247
00248
00249
00250 void CurveFitter::
00251 sort_points() {
00252 sort(_data.begin(), _data.end());
00253 }
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263 void CurveFitter::
00264 desample(PN_stdfloat factor) {
00265 int in, out;
00266 PN_stdfloat count = factor;
00267
00268 out = 0;
00269 for (in = 0; in < (int)_data.size()-1; in++) {
00270 if (count >= factor) {
00271 _data[out] = _data[in];
00272 out++;
00273 count -= factor;
00274 }
00275 count += 1.0f;
00276 }
00277
00278 _data[out] = _data.back();
00279 out++;
00280
00281 _data.erase(_data.begin() + out, _data.end());
00282 }
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293 void CurveFitter::
00294 compute_tangents(PN_stdfloat scale) {
00295
00296 bool closed = false;
00297
00298 if (_got_xyz) {
00299 closed =
00300 (_data.front()._xyz.almost_equal(_data.back()._xyz, 0.001f));
00301
00302 } else if (_got_hpr) {
00303 closed =
00304 (_data.front()._hpr.almost_equal(_data.back()._hpr, 0.001f));
00305 }
00306
00307 int i;
00308 int len = _data.size();
00309
00310
00311
00312
00313 if (_got_xyz) {
00314 for (i = 1; i < len-1; i++) {
00315 _data[i]._tangent =
00316 (_data[i+1]._xyz - _data[i-1]._xyz) * scale /
00317 (_data[i+1]._t - _data[i-1]._t);
00318 }
00319 }
00320 if (_got_hpr) {
00321 for (i = 1; i < len-1; i++) {
00322 _data[i]._hpr_tangent =
00323 (_data[i+1]._hpr - _data[i-1]._hpr) * scale /
00324 (_data[i+1]._t - _data[i-1]._t);
00325 }
00326 }
00327
00328
00329 if (closed) {
00330 if (_got_xyz) {
00331 _data[0]._tangent = _data[len-1]._tangent =
00332 (_data[1]._xyz - _data[len-2]._xyz) * scale /
00333 ((_data[1]._t - _data[0]._t) + (_data[len-1]._t - _data[len-2]._t));
00334 }
00335 if (_got_hpr) {
00336 _data[0]._tangent = _data[len-1]._tangent =
00337 (_data[1]._hpr - _data[len-2]._hpr) * scale /
00338 ((_data[1]._t - _data[0]._t) + (_data[len-1]._t - _data[len-2]._t));
00339 }
00340
00341 } else {
00342 if (_got_xyz) {
00343 _data[0]._tangent =
00344 (_data[1]._xyz - _data[0]._xyz) * scale /
00345 ((_data[1]._t - _data[0]._t) * 2.0f);
00346 _data[len-1]._tangent =
00347 (_data[len-1]._xyz - _data[len-2]._xyz) * scale /
00348 ((_data[len-1]._t - _data[len-2]._t) * 2.0f);
00349 }
00350 if (_got_hpr) {
00351 _data[0]._tangent =
00352 (_data[1]._hpr - _data[0]._hpr) * scale /
00353 ((_data[1]._t - _data[0]._t) * 2.0f);
00354 _data[len-1]._tangent =
00355 (_data[len-1]._hpr - _data[len-2]._hpr) * scale /
00356 ((_data[len-1]._t - _data[len-2]._t) * 2.0f);
00357 }
00358 }
00359 }
00360
00361
00362
00363
00364
00365
00366
00367 PT(ParametricCurveCollection) CurveFitter::
00368 make_hermite() const {
00369 PT(ParametricCurveCollection) result = new ParametricCurveCollection;
00370
00371 if (_got_xyz) {
00372 HermiteCurve *hc = new HermiteCurve;
00373 result->add_curve(hc);
00374 hc->set_curve_type(PCT_XYZ);
00375
00376 Data::const_iterator di;
00377 for (di = _data.begin(); di != _data.end(); ++di) {
00378 int n = hc->insert_cv((*di)._t);
00379 hc->set_cv_type(n, HC_SMOOTH);
00380 hc->set_cv_point(n, (*di)._xyz);
00381 hc->set_cv_in(n, (*di)._tangent);
00382 hc->set_cv_out(n, (*di)._tangent);
00383 }
00384 }
00385
00386 if (_got_hpr) {
00387 HermiteCurve *hc = new HermiteCurve;
00388 result->add_curve(hc);
00389 hc->set_curve_type(PCT_HPR);
00390
00391 Data::const_iterator di;
00392 for (di = _data.begin(); di != _data.end(); ++di) {
00393 int n = hc->insert_cv((*di)._t);
00394 hc->set_cv_type(n, HC_SMOOTH);
00395 hc->set_cv_point(n, (*di)._hpr);
00396 hc->set_cv_in(n, (*di)._hpr_tangent);
00397 hc->set_cv_out(n, (*di)._hpr_tangent);
00398 }
00399 }
00400
00401 return result;
00402 }
00403
00404
00405
00406
00407
00408
00409
00410
00411 PT(ParametricCurveCollection) CurveFitter::
00412 make_nurbs() const {
00413
00414
00415 PT(ParametricCurveCollection) hermites = make_hermite();
00416 PT(ParametricCurveCollection) result = new ParametricCurveCollection;
00417
00418 int num_curves = hermites->get_num_curves();
00419 for (int c = 0; c < num_curves; c++) {
00420 NurbsCurve *nc = new NurbsCurve(*hermites->get_curve(c));
00421 result->add_curve(nc);
00422
00423
00424
00425
00426 int num_knots = nc->get_num_knots();
00427
00428
00429
00430 assert(nc->get_order() == 4);
00431 assert(num_knots > 0);
00432
00433
00434
00435
00436
00437
00438
00439
00440 int i;
00441 PN_stdfloat k1, k2 = nc->get_knot(num_knots-1);
00442 const PN_stdfloat one_third = 1.0f/3.0f;
00443 for (i = 3; i < num_knots - 4; i += 3) {
00444 k1 = nc->get_knot(i-1);
00445 k2 = nc->get_knot(i+2);
00446 nc->set_knot(i, (k1 + k1 + k2) * one_third);
00447 nc->set_knot(i+1, (k1 + k2 + k2) * one_third);
00448 }
00449
00450
00451 nc->set_knot(num_knots-4, k2);
00452
00453
00454 nc->recompute();
00455 }
00456
00457 return result;
00458 }
00459
00460
00461
00462
00463
00464
00465 void CurveFitter::
00466 output(ostream &out) const {
00467 out << "CurveFitter, " << _data.size() << " samples.\n";
00468 }
00469
00470
00471
00472
00473
00474
00475 void CurveFitter::
00476 write(ostream &out) const {
00477 out << "CurveFitter, " << _data.size() << " samples:\n";
00478 Data::const_iterator di;
00479 for (di = _data.begin(); di != _data.end(); ++di) {
00480 out << " " << (*di) << "\n";
00481 }
00482 }
00483