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