Panda3D
|
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