Panda3D
Loading...
Searching...
No Matches
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
24TypeHandle CurveFitter::_type_handle;
25
26/**
27 *
28 */
29CurveFitter::
30CurveFitter() {
31 _got_xyz = false;
32 _got_hpr = false;
33}
34
35/**
36 *
37 */
38CurveFitter::
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 */
47reset() {
48 _data.erase(_data.begin(), _data.end());
49}
50
51/**
52 * Adds a single sample xyz.
53 */
55add_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 */
67add_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 */
79add_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 */
93get_num_samples() const {
94 return _data.size();
95}
96
97/**
98 * Returns the parametric value of the nth sample added.
99 */
100PN_stdfloat CurveFitter::
101get_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 */
110get_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 */
119get_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 */
129get_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 */
139remove_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 */
154sample(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 */
185wrap_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 */
215sort_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 */
225desample(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 */
251compute_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 */
320PT(ParametricCurveCollection) CurveFitter::
321make_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 */
361PT(ParametricCurveCollection) CurveFitter::
362make_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 */
413void CurveFitter::
414output(std::ostream &out) const {
415 out << "CurveFitter, " << _data.size() << " samples.\n";
416}
417
418/**
419 *
420 */
421void CurveFitter::
422write(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}
void add_xyz(PN_stdfloat t, const LVecBase3 &xyz)
Adds a single sample xyz.
PN_stdfloat get_sample_t(int n) const
Returns the parametric value of the nth sample added.
LVecBase3 get_sample_xyz(int n) const
Returns the point in space of the nth sample added.
void wrap_hpr()
Resets each HPR data point so that the maximum delta between any two consecutive points is 180 degree...
int get_num_samples() const
Returns the number of sample points that have been added.
void sort_points()
Sorts all the data points in order by parametric time, in case they were added in an incorrect order.
void sample(ParametricCurveCollection *curves, int count)
Generates a series of data points by sampling the given curve (or xyz/hpr curves) the indicated numbe...
void compute_tangents(PN_stdfloat scale)
Once a set of points has been built, and prior to calling MakeHermite() or MakeNurbs(),...
LVecBase3 get_sample_tangent(int n) const
Returns the tangent associated with the nth sample added.
void remove_samples(int begin, int end)
Eliminates all samples from index begin, up to but not including index end, from the database.
void add_hpr(PN_stdfloat t, const LVecBase3 &hpr)
Adds a single sample hpr.
void add_xyz_hpr(PN_stdfloat t, const LVecBase3 &xyz, const LVecBase3 &hpr)
Adds a single sample xyz & hpr simultaneously.
LVecBase3 get_sample_hpr(int n) const
Returns the orientation of the nth sample added.
void desample(PN_stdfloat factor)
Removes sample points in order to reduce the complexity of a sampled curve.
void reset()
Removes all the data points previously added to the CurveFitter, and initializes it for a new curve.
A parametric curve defined by a sequence of control vertices, each with an in and out tangent.
bool set_cv_out(int n, PN_stdfloat x, PN_stdfloat y, PN_stdfloat z)
Changes the given CV's out tangent.
int insert_cv(PN_stdfloat t)
Inserts a new CV at the given parametric point along the curve.
bool set_cv_point(int n, PN_stdfloat x, PN_stdfloat y, PN_stdfloat z)
Changes the given CV's position.
bool set_cv_in(int n, PN_stdfloat x, PN_stdfloat y, PN_stdfloat z)
Changes the given CV's in tangent.
bool set_cv_type(int n, int type)
Changes the given CV's continuity type.
A Nonuniform Rational B-Spline.
Definition nurbsCurve.h:41
virtual bool recompute()
Recalculates the curve basis according to the latest position of the CV's, knots, etc.
virtual PN_stdfloat get_knot(int n) const
Retrieves the value of the indicated knot.
virtual int get_num_knots() const
Returns the number of knots on the curve.
virtual bool set_knot(int n, PN_stdfloat t)
Sets the value of the indicated knot.
This is a set of zero or more ParametricCurves, which may or may not be related.
get_xyz_curve
Returns the first XYZ curve in the collection, if any, or NULL if there are none.
get_num_curves
Returns the number of ParametricCurves in the collection.
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...
get_hpr_curve
Returns the first HPR curve in the collection, if any, or NULL if there are none.
get_max_t
Returns the maximum T value associated with the *last* curve in the collection.
void set_curve_type(int type)
Sets the flag indicating the use to which the curve is intended to be put.
TypeHandle is the identifier used to differentiate C++ class types.
Definition typeHandle.h:81
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.
PANDA 3D SOFTWARE Copyright (c) Carnegie Mellon University.