Panda3D
 All Classes Functions Variables Enumerations
curveFitter.cxx
1 // Filename: curveFitter.cxx
2 // Created by: drose (17Sep98)
3 //
4 ////////////////////////////////////////////////////////////////////
5 //
6 // PANDA 3D SOFTWARE
7 // Copyright (c) Carnegie Mellon University. All rights reserved.
8 //
9 // All use of this software is subject to the terms of the revised BSD
10 // license. You should have received a copy of this license along
11 // with this source code in a file named "LICENSE."
12 //
13 ////////////////////////////////////////////////////////////////////
14 
15 #include "pandabase.h"
16 #include "pointerTo.h"
17 
18 #include "curveFitter.h"
19 #include "config_parametrics.h"
20 #include "parametricCurve.h"
21 #include "nurbsCurve.h"
22 #include "hermiteCurve.h"
23 #include <algorithm>
24 
25 TypeHandle CurveFitter::_type_handle;
26 
27 ////////////////////////////////////////////////////////////////////
28 // Function: CurveFitter::Constructor
29 // Access: Public
30 // Description:
31 ////////////////////////////////////////////////////////////////////
32 CurveFitter::
33 CurveFitter() {
34  _got_xyz = false;
35  _got_hpr = false;
36 }
37 
38 ////////////////////////////////////////////////////////////////////
39 // Function: CurveFitter::Destructor
40 // Access: Public
41 // Description:
42 ////////////////////////////////////////////////////////////////////
43 CurveFitter::
44 ~CurveFitter() {
45 }
46 
47 ////////////////////////////////////////////////////////////////////
48 // Function: CurveFitter::reset
49 // Access: Public
50 // Description: Removes all the data points previously added to the
51 // CurveFitter, and initializes it for a new curve.
52 ////////////////////////////////////////////////////////////////////
53 void CurveFitter::
54 reset() {
55  _data.erase(_data.begin(), _data.end());
56 }
57 
58 ////////////////////////////////////////////////////////////////////
59 // Function: CurveFitter::add_xyz
60 // Access: Public
61 // Description: Adds a single sample xyz.
62 ////////////////////////////////////////////////////////////////////
63 void CurveFitter::
64 add_xyz(PN_stdfloat t, const LVecBase3 &xyz) {
65  DataPoint dp;
66  dp._t = t;
67  dp._xyz = xyz;
68  _data.push_back(dp);
69  _got_xyz = true;
70 }
71 
72 ////////////////////////////////////////////////////////////////////
73 // Function: CurveFitter::add_hpr
74 // Access: Public
75 // Description: Adds a single sample hpr.
76 ////////////////////////////////////////////////////////////////////
77 void CurveFitter::
78 add_hpr(PN_stdfloat t, const LVecBase3 &hpr) {
79  DataPoint dp;
80  dp._t = t;
81  dp._hpr = hpr;
82  _data.push_back(dp);
83  _got_hpr = true;
84 }
85 
86 ////////////////////////////////////////////////////////////////////
87 // Function: CurveFitter::add_xyz_hpr
88 // Access: Public
89 // Description: Adds a single sample xyz & hpr simultaneously.
90 ////////////////////////////////////////////////////////////////////
91 void CurveFitter::
92 add_xyz_hpr(PN_stdfloat t, const LVecBase3 &xyz, const LVecBase3 &hpr) {
93  DataPoint dp;
94  dp._t = t;
95  dp._xyz = xyz;
96  dp._hpr = hpr;
97  _data.push_back(dp);
98  _got_xyz = true;
99  _got_hpr = true;
100 }
101 
102 ////////////////////////////////////////////////////////////////////
103 // Function: CurveFitter::get_num_samples
104 // Access: Public
105 // Description: Returns the number of sample points that have been
106 // added.
107 ////////////////////////////////////////////////////////////////////
108 int CurveFitter::
110  return _data.size();
111 }
112 
113 ////////////////////////////////////////////////////////////////////
114 // Function: CurveFitter::get_sample_t
115 // Access: Public
116 // Description: Returns the parametric value of the nth sample added.
117 ////////////////////////////////////////////////////////////////////
118 PN_stdfloat CurveFitter::
119 get_sample_t(int n) const {
120  nassertr(n >= 0 && n < (int)_data.size(), 0.0f);
121  return _data[n]._t;
122 }
123 
124 ////////////////////////////////////////////////////////////////////
125 // Function: CurveFitter::get_sample_xyz
126 // Access: Public
127 // Description: Returns the point in space of the nth sample added.
128 ////////////////////////////////////////////////////////////////////
130 get_sample_xyz(int n) const {
131  nassertr(n >= 0 && n < (int)_data.size(), LVecBase3::zero());
132  return _data[n]._xyz;
133 }
134 
135 ////////////////////////////////////////////////////////////////////
136 // Function: CurveFitter::get_sample_hpr
137 // Access: Public
138 // Description: Returns the orientation of the nth sample added.
139 ////////////////////////////////////////////////////////////////////
141 get_sample_hpr(int n) const {
142  nassertr(n >= 0 && n < (int)_data.size(), LVecBase3::zero());
143  return _data[n]._hpr;
144 }
145 
146 ////////////////////////////////////////////////////////////////////
147 // Function: CurveFitter::get_sample_tangent
148 // Access: Public
149 // Description: Returns the tangent associated with the nth sample
150 // added. This is only meaningful if compute_tangents()
151 // has already been called.
152 ////////////////////////////////////////////////////////////////////
154 get_sample_tangent(int n) const {
155  nassertr(n >= 0 && n < (int)_data.size(), LVecBase3::zero());
156  return _data[n]._tangent;
157 }
158 
159 ////////////////////////////////////////////////////////////////////
160 // Function: CurveFitter::remove_samples
161 // Access: Public
162 // Description: Eliminates all samples from index begin, up to but not
163 // including index end, from the database.
164 ////////////////////////////////////////////////////////////////////
165 void CurveFitter::
166 remove_samples(int begin, int end) {
167  begin = max(0, min((int)_data.size(), begin));
168  end = max(0, min((int)_data.size(), end));
169 
170  nassertv(begin <= end);
171 
172  _data.erase(_data.begin() + begin, _data.begin() + end);
173 }
174 
175 ////////////////////////////////////////////////////////////////////
176 // Function: CurveFitter::sample
177 // Access: Public
178 // Description: Generates a series of data points by sampling the
179 // given curve (or xyz/hpr curves) the indicated number
180 // of times. The sampling is made evenly in parametric
181 // time, and then the timewarps, if any, are applied.
182 ////////////////////////////////////////////////////////////////////
183 void CurveFitter::
184 sample(ParametricCurveCollection *curves, int count) {
185  nassertv(curves != (ParametricCurveCollection *)NULL);
186  PN_stdfloat max_t = curves->get_max_t();
187  PN_stdfloat t, last_t, d;
188  DataPoint dp;
189 
190  last_t = 0.0f;
191  d = 0.0f;
192  int i;
193  for (i = 0; i < count; i++) {
194  t = max_t * (PN_stdfloat)i / (PN_stdfloat)(count-1);
195  if (curves->evaluate(t, dp._xyz, dp._hpr)) {
196  dp._t = t;
197  _data.push_back(dp);
198  }
199  }
200 
201  if (curves->get_xyz_curve() != (ParametricCurve *)NULL) {
202  _got_xyz = true;
203  }
204  if (curves->get_hpr_curve() != (ParametricCurve *)NULL) {
205  _got_hpr = true;
206  }
207 }
208 
209 
210 
211 ////////////////////////////////////////////////////////////////////
212 // Function: CurveFitter::wrap_hpr
213 // Access: Public
214 // Description: Resets each HPR data point so that the maximum delta
215 // between any two consecutive points is 180 degrees,
216 // which should prevent incorrect HPR wrapping.
217 ////////////////////////////////////////////////////////////////////
218 void CurveFitter::
220  Data::iterator di;
221  LVecBase3 last(0.0f, 0.0f, 0.0f);
222  LVecBase3 net(0.0f, 0.0f, 0.0f);
223 
224  for (di = _data.begin(); di != _data.end(); ++di) {
225  int i;
226  for (i = 0; i < 3; i++) {
227  (*di)._hpr[i] += net[i];
228 
229  while (((*di)._hpr[i] - last[i]) > 180.0f) {
230  (*di)._hpr[i] -= 360.0f;
231  net[i] -= 360.0f;
232  }
233 
234  while (((*di)._hpr[i] - last[i]) < -180.0f) {
235  (*di)._hpr[i] += 360.0f;
236  net[i] += 360.0f;
237  }
238 
239  last[i] = (*di)._hpr[i];
240  }
241  }
242 }
243 
244 ////////////////////////////////////////////////////////////////////
245 // Function: CurveFitter::sort_points
246 // Access: Public
247 // Description: Sorts all the data points in order by parametric
248 // time, in case they were added in an incorrect order.
249 ////////////////////////////////////////////////////////////////////
250 void CurveFitter::
252  sort(_data.begin(), _data.end());
253 }
254 
255 ////////////////////////////////////////////////////////////////////
256 // Function: CurveFitter::desample
257 // Access: Public
258 // Description: Removes sample points in order to reduce the
259 // complexity of a sampled curve. Keeps one out of
260 // every factor samples. Also keeps the first and the
261 // last samples.
262 ////////////////////////////////////////////////////////////////////
263 void CurveFitter::
264 desample(PN_stdfloat factor) {
265  int in, out;
266  PN_stdfloat count = factor;
267 
268  out = 0;
269  for (in = 0; in < (int)_data.size()-1; in++) {
270  if (count >= factor) {
271  _data[out] = _data[in];
272  out++;
273  count -= factor;
274  }
275  count += 1.0f;
276  }
277 
278  _data[out] = _data.back();
279  out++;
280 
281  _data.erase(_data.begin() + out, _data.end());
282 }
283 
284 ////////////////////////////////////////////////////////////////////
285 // Function: CurveFitter::compute_tangents
286 // Access: Public
287 // Description: Once a set of points has been built, and prior to
288 // calling MakeHermite() or MakeNurbs(),
289 // ComputeTangents() must be called to set up the
290 // tangents correctly (unless the tangents were defined
291 // as the points were added).
292 ////////////////////////////////////////////////////////////////////
293 void CurveFitter::
294 compute_tangents(PN_stdfloat scale) {
295  // If the head and tail points match up, close the curve.
296  bool closed = false;
297 
298  if (_got_xyz) {
299  closed =
300  (_data.front()._xyz.almost_equal(_data.back()._xyz, 0.001f));
301 
302  } else if (_got_hpr) {
303  closed =
304  (_data.front()._hpr.almost_equal(_data.back()._hpr, 0.001f));
305  }
306 
307  int i;
308  int len = _data.size();
309 
310  // First, get all the points in the middle, excluding endpoints.
311  // These are handled the same whether we are closing the curve or
312  // not.
313  if (_got_xyz) {
314  for (i = 1; i < len-1; i++) {
315  _data[i]._tangent =
316  (_data[i+1]._xyz - _data[i-1]._xyz) * scale /
317  (_data[i+1]._t - _data[i-1]._t);
318  }
319  }
320  if (_got_hpr) {
321  for (i = 1; i < len-1; i++) {
322  _data[i]._hpr_tangent =
323  (_data[i+1]._hpr - _data[i-1]._hpr) * scale /
324  (_data[i+1]._t - _data[i-1]._t);
325  }
326  }
327 
328  // Now handle the endpoints.
329  if (closed) {
330  if (_got_xyz) {
331  _data[0]._tangent = _data[len-1]._tangent =
332  (_data[1]._xyz - _data[len-2]._xyz) * scale /
333  ((_data[1]._t - _data[0]._t) + (_data[len-1]._t - _data[len-2]._t));
334  }
335  if (_got_hpr) {
336  _data[0]._tangent = _data[len-1]._tangent =
337  (_data[1]._hpr - _data[len-2]._hpr) * scale /
338  ((_data[1]._t - _data[0]._t) + (_data[len-1]._t - _data[len-2]._t));
339  }
340 
341  } else {
342  if (_got_xyz) {
343  _data[0]._tangent =
344  (_data[1]._xyz - _data[0]._xyz) * scale /
345  ((_data[1]._t - _data[0]._t) * 2.0f);
346  _data[len-1]._tangent =
347  (_data[len-1]._xyz - _data[len-2]._xyz) * scale /
348  ((_data[len-1]._t - _data[len-2]._t) * 2.0f);
349  }
350  if (_got_hpr) {
351  _data[0]._tangent =
352  (_data[1]._hpr - _data[0]._hpr) * scale /
353  ((_data[1]._t - _data[0]._t) * 2.0f);
354  _data[len-1]._tangent =
355  (_data[len-1]._hpr - _data[len-2]._hpr) * scale /
356  ((_data[len-1]._t - _data[len-2]._t) * 2.0f);
357  }
358  }
359 }
360 
361 ////////////////////////////////////////////////////////////////////
362 // Function: CurveFitter::make_hermite
363 // Access: Public
364 // Description: Converts the current set of data points into a
365 // Hermite curve.
366 ////////////////////////////////////////////////////////////////////
368 make_hermite() const {
370 
371  if (_got_xyz) {
372  HermiteCurve *hc = new HermiteCurve;
373  result->add_curve(hc);
374  hc->set_curve_type(PCT_XYZ);
375 
376  Data::const_iterator di;
377  for (di = _data.begin(); di != _data.end(); ++di) {
378  int n = hc->insert_cv((*di)._t);
379  hc->set_cv_type(n, HC_SMOOTH);
380  hc->set_cv_point(n, (*di)._xyz);
381  hc->set_cv_in(n, (*di)._tangent);
382  hc->set_cv_out(n, (*di)._tangent);
383  }
384  }
385 
386  if (_got_hpr) {
387  HermiteCurve *hc = new HermiteCurve;
388  result->add_curve(hc);
389  hc->set_curve_type(PCT_HPR);
390 
391  Data::const_iterator di;
392  for (di = _data.begin(); di != _data.end(); ++di) {
393  int n = hc->insert_cv((*di)._t);
394  hc->set_cv_type(n, HC_SMOOTH);
395  hc->set_cv_point(n, (*di)._hpr);
396  hc->set_cv_in(n, (*di)._hpr_tangent);
397  hc->set_cv_out(n, (*di)._hpr_tangent);
398  }
399  }
400 
401  return result;
402 }
403 
404 ////////////////////////////////////////////////////////////////////
405 // Function: CurveFitter::make_nurbs
406 // Access: Public
407 // Description: Converts the current set of data points into a
408 // NURBS curve. This gives a smoother curve than
409 // produced by MakeHermite().
410 ////////////////////////////////////////////////////////////////////
411 PT(ParametricCurveCollection) CurveFitter::
412 make_nurbs() const {
413  // We start with the HermiteCurves produced above, then convert them
414  // to NURBS form.
415  PT(ParametricCurveCollection) hermites = make_hermite();
416  PT(ParametricCurveCollection) result = new ParametricCurveCollection;
417 
418  int num_curves = hermites->get_num_curves();
419  for (int c = 0; c < num_curves; c++) {
420  NurbsCurve *nc = new NurbsCurve(*hermites->get_curve(c));
421  result->add_curve(nc);
422 
423  // Now we even out the knots to smooth out the curve and make
424  // everything c2 continuous.
425 
426  int num_knots = nc->get_num_knots();
427 
428  // We expect this to be a 4th order curve, since we just converted
429  // it from a Hermite.
430  assert(nc->get_order() == 4);
431  assert(num_knots > 0);
432 
433  // Now the knot sequence goes something like this:
434  // 0 0 0 0 1 1 1 2 2 2 3 3 3 4 4 4 4
435 
436  // We'll consider pairs of knot values beginning at position 3 and
437  // every third position thereafter. We just even out these values
438  // between their two neighbors.
439 
440  int i;
441  PN_stdfloat k1, k2 = nc->get_knot(num_knots-1);
442  const PN_stdfloat one_third = 1.0f/3.0f;
443  for (i = 3; i < num_knots - 4; i += 3) {
444  k1 = nc->get_knot(i-1);
445  k2 = nc->get_knot(i+2);
446  nc->set_knot(i, (k1 + k1 + k2) * one_third);
447  nc->set_knot(i+1, (k1 + k2 + k2) * one_third);
448  }
449 
450  // The last knot must have the terminal value.
451  nc->set_knot(num_knots-4, k2);
452 
453  // Finally, recompute the curve.
454  nc->recompute();
455  }
456 
457  return result;
458 }
459 
460 ////////////////////////////////////////////////////////////////////
461 // Function: CurveFitter::output
462 // Access: Public
463 // Description:
464 ////////////////////////////////////////////////////////////////////
465 void CurveFitter::
466 output(ostream &out) const {
467  out << "CurveFitter, " << _data.size() << " samples.\n";
468 }
469 
470 ////////////////////////////////////////////////////////////////////
471 // Function: CurveFitter::write
472 // Access: Public
473 // Description:
474 ////////////////////////////////////////////////////////////////////
475 void CurveFitter::
476 write(ostream &out) const {
477  out << "CurveFitter, " << _data.size() << " samples:\n";
478  Data::const_iterator di;
479  for (di = _data.begin(); di != _data.end(); ++di) {
480  out << " " << (*di) << "\n";
481  }
482 }
483 
This is the base class for all three-component vectors and points.
Definition: lvecBase3.h:105
LVecBase3 get_sample_xyz(int n) const
Returns the point in space of the nth sample added.
ParametricCurve * get_hpr_curve() const
Returns the first HPR curve in the collection, if any, or NULL if there are none. ...
void desample(PN_stdfloat factor)
Removes sample points in order to reduce the complexity of a sampled curve.
A virtual base class for parametric curves.
int insert_cv(PN_stdfloat t)
Inserts a new CV at the given parametric point along the curve.
bool set_cv_out(int n, PN_stdfloat x, PN_stdfloat y, PN_stdfloat z)
Changes the given CV&#39;s out tangent.
PN_stdfloat get_sample_t(int n) const
Returns the parametric value of the nth sample added.
PN_stdfloat get_max_t() const
Returns the maximum T value associated with the last* curve in the collection.
static const LVecBase3f & zero()
Returns a zero-length vector.
Definition: lvecBase3.h:381
bool set_cv_point(int n, PN_stdfloat x, PN_stdfloat y, PN_stdfloat z)
Changes the given CV&#39;s position.
A parametric curve defined by a sequence of control vertices, each with an in and out tangent...
Definition: hermiteCurve.h:88
ParametricCurve * get_xyz_curve() const
Returns the first XYZ curve in the collection, if any, or NULL if there are none. ...
void compute_tangents(PN_stdfloat scale)
Once a set of points has been built, and prior to calling MakeHermite() or MakeNurbs(), ComputeTangents() must be called to set up the tangents correctly (unless the tangents were defined as the points were added).
LVecBase3 get_sample_tangent(int n) const
Returns the tangent associated with the nth sample added.
void add_xyz_hpr(PN_stdfloat t, const LVecBase3 &xyz, const LVecBase3 &hpr)
Adds a single sample xyz &amp; hpr simultaneously.
Definition: curveFitter.cxx:92
This is a set of zero or more ParametricCurves, which may or may not be related.
virtual bool set_knot(int n, PN_stdfloat t)
Sets the value of the indicated knot.
Definition: nurbsCurve.cxx:274
LVecBase3 get_sample_hpr(int n) const
Returns the orientation of the nth sample added.
int get_num_samples() const
Returns the number of sample points that have been added.
void wrap_hpr()
Resets each HPR data point so that the maximum delta between any two consecutive points is 180 degree...
virtual bool recompute()
Recalculates the curve basis according to the latest position of the CV&#39;s, knots, etc...
Definition: nurbsCurve.cxx:312
A Nonuniform Rational B-Spline.
Definition: nurbsCurve.h:48
void add_hpr(PN_stdfloat t, const LVecBase3 &hpr)
Adds a single sample hpr.
Definition: curveFitter.cxx:78
bool set_cv_type(int n, int type)
Changes the given CV&#39;s continuity type.
void add_xyz(PN_stdfloat t, const LVecBase3 &xyz)
Adds a single sample xyz.
Definition: curveFitter.cxx:64
virtual PN_stdfloat get_knot(int n) const
Retrieves the value of the indicated knot.
Definition: nurbsCurve.cxx:290
void set_curve_type(int type)
Sets the flag indicating the use to which the curve is intended to be put.
void sort_points()
Sorts all the data points in order by parametric time, in case they were added in an incorrect order...
void remove_samples(int begin, int end)
Eliminates all samples from index begin, up to but not including index end, from the database...
bool evaluate(PN_stdfloat t, LVecBase3 &xyz, LVecBase3 &hpr) const
Computes the position and rotation represented by the first XYZ and HPR curves in the collection at t...
void sample(ParametricCurveCollection *curves, int count)
Generates a series of data points by sampling the given curve (or xyz/hpr curves) the indicated numbe...
TypeHandle is the identifier used to differentiate C++ class types.
Definition: typeHandle.h:85
bool set_cv_in(int n, PN_stdfloat x, PN_stdfloat y, PN_stdfloat z)
Changes the given CV&#39;s in tangent.
void reset()
Removes all the data points previously added to the CurveFitter, and initializes it for a new curve...
Definition: curveFitter.cxx:54
virtual int get_num_knots() const
Returns the number of knots on the curve.
Definition: nurbsCurve.cxx:141