Panda3D

curveFitter.cxx

00001 // Filename: curveFitter.cxx
00002 // Created by:  drose (17Sep98)
00003 //
00004 ////////////////////////////////////////////////////////////////////
00005 //
00006 // PANDA 3D SOFTWARE
00007 // Copyright (c) Carnegie Mellon University.  All rights reserved.
00008 //
00009 // All use of this software is subject to the terms of the revised BSD
00010 // license.  You should have received a copy of this license along
00011 // with this source code in a file named "LICENSE."
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 //     Function: CurveFitter::Constructor
00029 //       Access: Public
00030 //  Description:
00031 ////////////////////////////////////////////////////////////////////
00032 CurveFitter::
00033 CurveFitter() {
00034   _got_xyz = false;
00035   _got_hpr = false;
00036 }
00037 
00038 ////////////////////////////////////////////////////////////////////
00039 //     Function: CurveFitter::Destructor
00040 //       Access: Public
00041 //  Description:
00042 ////////////////////////////////////////////////////////////////////
00043 CurveFitter::
00044 ~CurveFitter() {
00045 }
00046 
00047 ////////////////////////////////////////////////////////////////////
00048 //     Function: CurveFitter::reset
00049 //       Access: Public
00050 //  Description: Removes all the data points previously added to the
00051 //               CurveFitter, and initializes it for a new curve.
00052 ////////////////////////////////////////////////////////////////////
00053 void CurveFitter::
00054 reset() {
00055   _data.erase(_data.begin(), _data.end());
00056 }
00057 
00058 ////////////////////////////////////////////////////////////////////
00059 //     Function: CurveFitter::add_xyz
00060 //       Access: Public
00061 //  Description: Adds a single sample xyz.
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 //     Function: CurveFitter::add_hpr
00074 //       Access: Public
00075 //  Description: Adds a single sample hpr.
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 //     Function: CurveFitter::add_xyz_hpr
00088 //       Access: Public
00089 //  Description: Adds a single sample xyz & hpr simultaneously.
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 //     Function: CurveFitter::get_num_samples
00104 //       Access: Public
00105 //  Description: Returns the number of sample points that have been
00106 //               added.
00107 ////////////////////////////////////////////////////////////////////
00108 int CurveFitter::
00109 get_num_samples() const {
00110   return _data.size();
00111 }
00112 
00113 ////////////////////////////////////////////////////////////////////
00114 //     Function: CurveFitter::get_sample_t
00115 //       Access: Public
00116 //  Description: Returns the parametric value of the nth sample added.
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 //     Function: CurveFitter::get_sample_xyz
00126 //       Access: Public
00127 //  Description: Returns the point in space of the nth sample added.
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 //     Function: CurveFitter::get_sample_hpr
00137 //       Access: Public
00138 //  Description: Returns the orientation of the nth sample added.
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 //     Function: CurveFitter::get_sample_tangent
00148 //       Access: Public
00149 //  Description: Returns the tangent associated with the nth sample
00150 //               added.  This is only meaningful if compute_tangents()
00151 //               has already been called.
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 //     Function: CurveFitter::remove_samples
00161 //       Access: Public
00162 //  Description: Eliminates all samples from index begin, up to but not
00163 //               including index end, from the database.
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 //     Function: CurveFitter::sample
00177 //       Access: Public
00178 //  Description: Generates a series of data points by sampling the
00179 //               given curve (or xyz/hpr curves) the indicated number
00180 //               of times.  The sampling is made evenly in parametric
00181 //               time, and then the timewarps, if any, are applied.
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 //     Function: CurveFitter::wrap_hpr
00213 //       Access: Public
00214 //  Description: Resets each HPR data point so that the maximum delta
00215 //               between any two consecutive points is 180 degrees,
00216 //               which should prevent incorrect HPR wrapping.
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 //     Function: CurveFitter::sort_points
00246 //       Access: Public
00247 //  Description: Sorts all the data points in order by parametric
00248 //               time, in case they were added in an incorrect order.
00249 ////////////////////////////////////////////////////////////////////
00250 void CurveFitter::
00251 sort_points() {
00252   sort(_data.begin(), _data.end());
00253 }
00254 
00255 ////////////////////////////////////////////////////////////////////
00256 //     Function: CurveFitter::desample
00257 //       Access: Public
00258 //  Description: Removes sample points in order to reduce the
00259 //               complexity of a sampled curve.  Keeps one out of
00260 //               every factor samples.  Also keeps the first and the
00261 //               last samples.
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 //     Function: CurveFitter::compute_tangents
00286 //       Access: Public
00287 //  Description: Once a set of points has been built, and prior to
00288 //               calling MakeHermite() or MakeNurbs(),
00289 //               ComputeTangents() must be called to set up the
00290 //               tangents correctly (unless the tangents were defined
00291 //               as the points were added).
00292 ////////////////////////////////////////////////////////////////////
00293 void CurveFitter::
00294 compute_tangents(PN_stdfloat scale) {
00295   // If the head and tail points match up, close the curve.
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   // First, get all the points in the middle, excluding endpoints.
00311   // These are handled the same whether we are closing the curve or
00312   // not.
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   // Now handle the endpoints.
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 //     Function: CurveFitter::make_hermite
00363 //       Access: Public
00364 //  Description: Converts the current set of data points into a
00365 //               Hermite curve.
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 //     Function: CurveFitter::make_nurbs
00406 //       Access: Public
00407 //  Description: Converts the current set of data points into a
00408 //               NURBS curve.  This gives a smoother curve than
00409 //               produced by MakeHermite().
00410 ////////////////////////////////////////////////////////////////////
00411 PT(ParametricCurveCollection) CurveFitter::
00412 make_nurbs() const {
00413   // We start with the HermiteCurves produced above, then convert them
00414   // to NURBS form.
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     // Now we even out the knots to smooth out the curve and make
00424     // everything c2 continuous.
00425 
00426     int num_knots = nc->get_num_knots();
00427 
00428     // We expect this to be a 4th order curve, since we just converted
00429     // it from a Hermite.
00430     assert(nc->get_order() == 4);
00431     assert(num_knots > 0);
00432 
00433     // Now the knot sequence goes something like this:
00434     //    0 0 0 0 1 1 1 2 2 2 3 3 3 4 4 4 4
00435 
00436     // We'll consider pairs of knot values beginning at position 3 and
00437     // every third position thereafter.  We just even out these values
00438     // between their two neighbors.
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     // The last knot must have the terminal value.
00451     nc->set_knot(num_knots-4, k2);
00452 
00453     // Finally, recompute the curve.
00454     nc->recompute();
00455   }
00456 
00457   return result;
00458 }
00459 
00460 ////////////////////////////////////////////////////////////////////
00461 //     Function: CurveFitter::output
00462 //       Access: Public
00463 //  Description:
00464 ////////////////////////////////////////////////////////////////////
00465 void CurveFitter::
00466 output(ostream &out) const {
00467   out << "CurveFitter, " << _data.size() << " samples.\n";
00468 }
00469 
00470 ////////////////////////////////////////////////////////////////////
00471 //     Function: CurveFitter::write
00472 //       Access: Public
00473 //  Description:
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 
 All Classes Functions Variables Enumerations