488 lines
15 KiB
C++
488 lines
15 KiB
C++
// Filename: curveFitter.cxx
|
|
// Created by: drose (17Sep98)
|
|
//
|
|
////////////////////////////////////////////////////////////////////
|
|
//
|
|
// PANDA 3D SOFTWARE
|
|
// Copyright (c) 2001, Disney Enterprises, Inc. All rights reserved
|
|
//
|
|
// All use of this software is subject to the terms of the Panda 3d
|
|
// Software license. You should have received a copy of this license
|
|
// along with this source code; you will also find a current copy of
|
|
// the license at http://www.panda3d.org/license.txt .
|
|
//
|
|
// To contact the maintainers of this program write to
|
|
// panda3d@yahoogroups.com .
|
|
//
|
|
////////////////////////////////////////////////////////////////////
|
|
|
|
#include "pandabase.h"
|
|
#include "pointerTo.h"
|
|
|
|
#include "curveFitter.h"
|
|
#include "config_parametrics.h"
|
|
#include "parametricCurve.h"
|
|
#include "classicNurbsCurve.h"
|
|
#include "hermiteCurve.h"
|
|
#include <algorithm>
|
|
|
|
TypeHandle CurveFitter::_type_handle;
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::Constructor
|
|
// Access: Public
|
|
// Description:
|
|
////////////////////////////////////////////////////////////////////
|
|
CurveFitter::
|
|
CurveFitter() {
|
|
_got_xyz = false;
|
|
_got_hpr = false;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::Destructor
|
|
// Access: Public
|
|
// Description:
|
|
////////////////////////////////////////////////////////////////////
|
|
CurveFitter::
|
|
~CurveFitter() {
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::reset
|
|
// Access: Public
|
|
// Description: Removes all the data points previously added to the
|
|
// CurveFitter, and initializes it for a new curve.
|
|
////////////////////////////////////////////////////////////////////
|
|
void CurveFitter::
|
|
reset() {
|
|
_data.erase(_data.begin(), _data.end());
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::add_xyz
|
|
// Access: Public
|
|
// Description: Adds a single sample xyz.
|
|
////////////////////////////////////////////////////////////////////
|
|
void CurveFitter::
|
|
add_xyz(float t, const LVecBase3f &xyz) {
|
|
DataPoint dp;
|
|
dp._t = t;
|
|
dp._xyz = xyz;
|
|
_data.push_back(dp);
|
|
_got_xyz = true;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::add_hpr
|
|
// Access: Public
|
|
// Description: Adds a single sample hpr.
|
|
////////////////////////////////////////////////////////////////////
|
|
void CurveFitter::
|
|
add_hpr(float t, const LVecBase3f &hpr) {
|
|
DataPoint dp;
|
|
dp._t = t;
|
|
dp._hpr = hpr;
|
|
_data.push_back(dp);
|
|
_got_hpr = true;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::add_xyz_hpr
|
|
// Access: Public
|
|
// Description: Adds a single sample xyz & hpr simultaneously.
|
|
////////////////////////////////////////////////////////////////////
|
|
void CurveFitter::
|
|
add_xyz_hpr(float t, const LVecBase3f &xyz, const LVecBase3f &hpr) {
|
|
DataPoint dp;
|
|
dp._t = t;
|
|
dp._xyz = xyz;
|
|
dp._hpr = hpr;
|
|
_data.push_back(dp);
|
|
_got_xyz = true;
|
|
_got_hpr = true;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::get_num_samples
|
|
// Access: Public
|
|
// Description: Returns the number of sample points that have been
|
|
// added.
|
|
////////////////////////////////////////////////////////////////////
|
|
int CurveFitter::
|
|
get_num_samples() const {
|
|
return _data.size();
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::get_sample_t
|
|
// Access: Public
|
|
// Description: Returns the parametric value of the nth sample added.
|
|
////////////////////////////////////////////////////////////////////
|
|
float CurveFitter::
|
|
get_sample_t(int n) const {
|
|
nassertr(n >= 0 && n < (int)_data.size(), 0.0f);
|
|
return _data[n]._t;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::get_sample_xyz
|
|
// Access: Public
|
|
// Description: Returns the point in space of the nth sample added.
|
|
////////////////////////////////////////////////////////////////////
|
|
LVecBase3f CurveFitter::
|
|
get_sample_xyz(int n) const {
|
|
nassertr(n >= 0 && n < (int)_data.size(), LVecBase3f::zero());
|
|
return _data[n]._xyz;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::get_sample_hpr
|
|
// Access: Public
|
|
// Description: Returns the orientation of the nth sample added.
|
|
////////////////////////////////////////////////////////////////////
|
|
LVecBase3f CurveFitter::
|
|
get_sample_hpr(int n) const {
|
|
nassertr(n >= 0 && n < (int)_data.size(), LVecBase3f::zero());
|
|
return _data[n]._hpr;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::get_sample_tangent
|
|
// Access: Public
|
|
// Description: Returns the tangent associated with the nth sample
|
|
// added. This is only meaningful if compute_tangents()
|
|
// has already been called.
|
|
////////////////////////////////////////////////////////////////////
|
|
LVecBase3f CurveFitter::
|
|
get_sample_tangent(int n) const {
|
|
nassertr(n >= 0 && n < (int)_data.size(), LVecBase3f::zero());
|
|
return _data[n]._tangent;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::remove_samples
|
|
// Access: Public
|
|
// Description: Eliminates all samples from index begin, up to but not
|
|
// including index end, from the database.
|
|
////////////////////////////////////////////////////////////////////
|
|
void CurveFitter::
|
|
remove_samples(int begin, int end) {
|
|
begin = max(0, min((int)_data.size(), begin));
|
|
end = max(0, min((int)_data.size(), end));
|
|
|
|
nassertv(begin <= end);
|
|
|
|
_data.erase(_data.begin() + begin, _data.begin() + end);
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::sample
|
|
// Access: Public
|
|
// Description: Generates a series of data points by sampling the
|
|
// given curve (or xyz/hpr curves) the indicated number
|
|
// of times. The sampling is made evenly in parametric
|
|
// time, and then the timewarps, if any, are applied.
|
|
////////////////////////////////////////////////////////////////////
|
|
void CurveFitter::
|
|
sample(ParametricCurveCollection *curves, int count) {
|
|
nassertv(curves != (ParametricCurveCollection *)NULL);
|
|
float max_t = curves->get_max_t();
|
|
float t, last_t, d;
|
|
DataPoint dp;
|
|
|
|
last_t = 0.0f;
|
|
d = 0.0f;
|
|
int i;
|
|
for (i = 0; i < count; i++) {
|
|
t = max_t * (float)i / (float)(count-1);
|
|
if (curves->evaluate(t, dp._xyz, dp._hpr)) {
|
|
dp._t = t;
|
|
_data.push_back(dp);
|
|
}
|
|
}
|
|
|
|
if (curves->get_xyz_curve() != (ParametricCurve *)NULL) {
|
|
_got_xyz = true;
|
|
}
|
|
if (curves->get_hpr_curve() != (ParametricCurve *)NULL) {
|
|
_got_hpr = true;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::wrap_hpr
|
|
// Access: Public
|
|
// Description: Resets each HPR data point so that the maximum delta
|
|
// between any two consecutive points is 180 degrees,
|
|
// which should prevent incorrect HPR wrapping.
|
|
////////////////////////////////////////////////////////////////////
|
|
void CurveFitter::
|
|
wrap_hpr() {
|
|
Data::iterator di;
|
|
LVecBase3f last(0.0f, 0.0f, 0.0f);
|
|
LVecBase3f net(0.0f, 0.0f, 0.0f);
|
|
|
|
for (di = _data.begin(); di != _data.end(); ++di) {
|
|
int i;
|
|
for (i = 0; i < 3; i++) {
|
|
(*di)._hpr[i] += net[i];
|
|
|
|
while (((*di)._hpr[i] - last[i]) > 180.0f) {
|
|
(*di)._hpr[i] -= 360.0f;
|
|
net[i] -= 360.0f;
|
|
}
|
|
|
|
while (((*di)._hpr[i] - last[i]) < -180.0f) {
|
|
(*di)._hpr[i] += 360.0f;
|
|
net[i] += 360.0f;
|
|
}
|
|
|
|
last[i] = (*di)._hpr[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::sort_points
|
|
// Access: Public
|
|
// Description: Sorts all the data points in order by parametric
|
|
// time, in case they were added in an incorrect order.
|
|
////////////////////////////////////////////////////////////////////
|
|
void CurveFitter::
|
|
sort_points() {
|
|
sort(_data.begin(), _data.end());
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::desample
|
|
// Access: Public
|
|
// Description: Removes sample points in order to reduce the
|
|
// complexity of a sampled curve. Keeps one out of
|
|
// every factor samples. Also keeps the first and the
|
|
// last samples.
|
|
////////////////////////////////////////////////////////////////////
|
|
void CurveFitter::
|
|
desample(float factor) {
|
|
int in, out;
|
|
float count = factor;
|
|
|
|
out = 0;
|
|
for (in = 0; in < (int)_data.size()-1; in++) {
|
|
if (count >= factor) {
|
|
_data[out] = _data[in];
|
|
out++;
|
|
count -= factor;
|
|
}
|
|
count += 1.0f;
|
|
}
|
|
|
|
_data[out] = _data.back();
|
|
out++;
|
|
|
|
_data.erase(_data.begin() + out, _data.end());
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::compute_tangents
|
|
// Access: Public
|
|
// Description: 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).
|
|
////////////////////////////////////////////////////////////////////
|
|
void CurveFitter::
|
|
compute_tangents(float scale) {
|
|
// If the head and tail points match up, close the curve.
|
|
bool closed = false;
|
|
|
|
if (_got_xyz) {
|
|
closed =
|
|
(_data.front()._xyz.almost_equal(_data.back()._xyz, 0.001f));
|
|
|
|
} else if (_got_hpr) {
|
|
closed =
|
|
(_data.front()._hpr.almost_equal(_data.back()._hpr, 0.001f));
|
|
}
|
|
|
|
int i;
|
|
int len = _data.size();
|
|
|
|
// First, get all the points in the middle, excluding endpoints.
|
|
// These are handled the same whether we are closing the curve or
|
|
// not.
|
|
if (_got_xyz) {
|
|
for (i = 1; i < len-1; i++) {
|
|
_data[i]._tangent =
|
|
(_data[i+1]._xyz - _data[i-1]._xyz) * scale /
|
|
(_data[i+1]._t - _data[i-1]._t);
|
|
}
|
|
}
|
|
if (_got_hpr) {
|
|
for (i = 1; i < len-1; i++) {
|
|
_data[i]._hpr_tangent =
|
|
(_data[i+1]._hpr - _data[i-1]._hpr) * scale /
|
|
(_data[i+1]._t - _data[i-1]._t);
|
|
}
|
|
}
|
|
|
|
// Now handle the endpoints.
|
|
if (closed) {
|
|
if (_got_xyz) {
|
|
_data[0]._tangent = _data[len-1]._tangent =
|
|
(_data[1]._xyz - _data[len-2]._xyz) * scale /
|
|
((_data[1]._t - _data[0]._t) + (_data[len-1]._t - _data[len-2]._t));
|
|
}
|
|
if (_got_hpr) {
|
|
_data[0]._tangent = _data[len-1]._tangent =
|
|
(_data[1]._hpr - _data[len-2]._hpr) * scale /
|
|
((_data[1]._t - _data[0]._t) + (_data[len-1]._t - _data[len-2]._t));
|
|
}
|
|
|
|
} else {
|
|
if (_got_xyz) {
|
|
_data[0]._tangent =
|
|
(_data[1]._xyz - _data[0]._xyz) * scale /
|
|
((_data[1]._t - _data[0]._t) * 2.0f);
|
|
_data[len-1]._tangent =
|
|
(_data[len-1]._xyz - _data[len-2]._xyz) * scale /
|
|
((_data[len-1]._t - _data[len-2]._t) * 2.0f);
|
|
}
|
|
if (_got_hpr) {
|
|
_data[0]._tangent =
|
|
(_data[1]._hpr - _data[0]._hpr) * scale /
|
|
((_data[1]._t - _data[0]._t) * 2.0f);
|
|
_data[len-1]._tangent =
|
|
(_data[len-1]._hpr - _data[len-2]._hpr) * scale /
|
|
((_data[len-1]._t - _data[len-2]._t) * 2.0f);
|
|
}
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::make_hermite
|
|
// Access: Public
|
|
// Description: Converts the current set of data points into a
|
|
// Hermite curve.
|
|
////////////////////////////////////////////////////////////////////
|
|
PT(ParametricCurveCollection) CurveFitter::
|
|
make_hermite() const {
|
|
PT(ParametricCurveCollection) result = new ParametricCurveCollection;
|
|
|
|
if (_got_xyz) {
|
|
HermiteCurve *hc = new HermiteCurve;
|
|
result->add_curve(hc);
|
|
hc->set_curve_type(PCT_XYZ);
|
|
|
|
Data::const_iterator di;
|
|
for (di = _data.begin(); di != _data.end(); ++di) {
|
|
int n = hc->insert_cv((*di)._t);
|
|
hc->set_cv_type(n, HC_SMOOTH);
|
|
hc->set_cv_point(n, (*di)._xyz);
|
|
hc->set_cv_in(n, (*di)._tangent);
|
|
hc->set_cv_out(n, (*di)._tangent);
|
|
}
|
|
}
|
|
|
|
if (_got_hpr) {
|
|
HermiteCurve *hc = new HermiteCurve;
|
|
result->add_curve(hc);
|
|
hc->set_curve_type(PCT_HPR);
|
|
|
|
Data::const_iterator di;
|
|
for (di = _data.begin(); di != _data.end(); ++di) {
|
|
int n = hc->insert_cv((*di)._t);
|
|
hc->set_cv_type(n, HC_SMOOTH);
|
|
hc->set_cv_point(n, (*di)._hpr);
|
|
hc->set_cv_in(n, (*di)._hpr_tangent);
|
|
hc->set_cv_out(n, (*di)._hpr_tangent);
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::make_nurbs
|
|
// Access: Public
|
|
// Description: Converts the current set of data points into a
|
|
// NURBS curve. This gives a smoother curve than
|
|
// produced by MakeHermite().
|
|
////////////////////////////////////////////////////////////////////
|
|
PT(ParametricCurveCollection) CurveFitter::
|
|
make_nurbs() const {
|
|
// We start with the HermiteCurves produced above, then convert them
|
|
// to NURBS form.
|
|
PT(ParametricCurveCollection) hermites = make_hermite();
|
|
PT(ParametricCurveCollection) result = new ParametricCurveCollection;
|
|
|
|
int num_curves = hermites->get_num_curves();
|
|
for (int c = 0; c < num_curves; c++) {
|
|
ClassicNurbsCurve *nc = new ClassicNurbsCurve(*hermites->get_curve(c));
|
|
result->add_curve(nc);
|
|
|
|
// Now we even out the knots to smooth out the curve and make
|
|
// everything c2 continuous.
|
|
|
|
int num_knots = nc->get_num_knots();
|
|
|
|
// We expect this to be a 4th order curve, since we just converted
|
|
// it from a Hermite.
|
|
assert(nc->get_order() == 4);
|
|
assert(num_knots > 0);
|
|
|
|
// Now the knot sequence goes something like this:
|
|
// 0 0 0 0 1 1 1 2 2 2 3 3 3 4 4 4 4
|
|
|
|
// We'll consider pairs of knot values beginning at position 3 and
|
|
// every third position thereafter. We just even out these values
|
|
// between their two neighbors.
|
|
|
|
int i;
|
|
float k1, k2 = nc->get_knot(num_knots-1);
|
|
const float one_third = 1.0f/3.0f;
|
|
for (i = 3; i < num_knots - 4; i += 3) {
|
|
k1 = nc->get_knot(i-1);
|
|
k2 = nc->get_knot(i+2);
|
|
nc->set_knot(i, (k1 + k1 + k2) * one_third);
|
|
nc->set_knot(i+1, (k1 + k2 + k2) * one_third);
|
|
}
|
|
|
|
// The last knot must have the terminal value.
|
|
nc->set_knot(num_knots-4, k2);
|
|
|
|
// Finally, recompute the curve.
|
|
nc->recompute();
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::output
|
|
// Access: Public
|
|
// Description:
|
|
////////////////////////////////////////////////////////////////////
|
|
void CurveFitter::
|
|
output(ostream &out) const {
|
|
out << "CurveFitter, " << _data.size() << " samples.\n";
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////
|
|
// Function: CurveFitter::write
|
|
// Access: Public
|
|
// Description:
|
|
////////////////////////////////////////////////////////////////////
|
|
void CurveFitter::
|
|
write(ostream &out) const {
|
|
out << "CurveFitter, " << _data.size() << " samples:\n";
|
|
Data::const_iterator di;
|
|
for (di = _data.begin(); di != _data.end(); ++di) {
|
|
out << " " << (*di) << "\n";
|
|
}
|
|
}
|
|
|