You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
757 lines
28 KiB
757 lines
28 KiB
//====== Copyright © 1996-2009, Valve Corporation, All rights reserved. =======
|
|
//
|
|
// A template implementation of a bezier curve class and associated helper
|
|
// functions.
|
|
//
|
|
//=============================================================================
|
|
|
|
#ifndef BEZIERCURVE_H
|
|
#define BEZIERCURVE_H
|
|
|
|
#ifdef _WIN32
|
|
#pragma once
|
|
#endif
|
|
|
|
|
|
const float ONE_THIRD = 1.0f / 3.0f;
|
|
const float TWO_THIRDS = 2.0f / 3.0f;
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Generic order N Bezier curve evaluation. Evaluates the bezier curve at the
|
|
// specified 0 to 1 parameter and returns the result.
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE, int ORDER >
|
|
struct BezierEvaluateImpl
|
|
{
|
|
// This generic implementation performs an iterative set of lerps in order
|
|
// to compute the bezier evaluation for any order curve, it is not efficiently
|
|
// and here primarily to maintain generality. All order curve that are used
|
|
// with any frequency should have their own specialized implementations.
|
|
static void BezierEvaluate( const POINT_TYPE *pControlPoints, float t, POINT_TYPE &result )
|
|
{
|
|
// This array is initialized with the control points and is then
|
|
// used to hold the intermediate results of each lerp in order
|
|
// to preserve the original set of control points.
|
|
POINT_TYPE points[ ORDER + 1 ];
|
|
|
|
for ( int i = 0; i <= ORDER; ++i )
|
|
{
|
|
points[ i ] = pControlPoints[ i ];
|
|
}
|
|
|
|
for ( int i = 1; i <= ORDER; ++i )
|
|
{
|
|
for ( int j = 0; j <= ( ORDER - i ); ++j )
|
|
{
|
|
points[ j ] = ( ( 1.0f - t ) * points[ j ] ) + ( t * points[ j + 1 ] );
|
|
}
|
|
}
|
|
|
|
result = points[ 0 ];
|
|
}
|
|
};
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Partial specialization for linear evaluation.
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE >
|
|
struct BezierEvaluateImpl< POINT_TYPE, 1 >
|
|
{
|
|
static void BezierEvaluate( const POINT_TYPE *points, float t, POINT_TYPE &result )
|
|
{
|
|
float u = 1.0f - t;
|
|
result = ( u * points[ 0 ] ) + ( t * points[ 1 ] );
|
|
}
|
|
};
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Partial specialization for quadratic bezier curve evaluation
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE >
|
|
struct BezierEvaluateImpl< POINT_TYPE, 2 >
|
|
{
|
|
static void BezierEvaluate( const POINT_TYPE *points, float t, POINT_TYPE &result )
|
|
{
|
|
float u = 1.0f - t;
|
|
float t2 = t * t;
|
|
float u2 = u * u;
|
|
result = ( u2 * points[ 0 ] ) + ( 2.0f * u * t * points[ 1 ] ) + ( t2 * points[ 2 ] );
|
|
}
|
|
};
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Partial specialization for cubic Bezier curve evaluation.
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE >
|
|
struct BezierEvaluateImpl< POINT_TYPE, 3 >
|
|
{
|
|
static void BezierEvaluate( const POINT_TYPE *points, float t, POINT_TYPE &result )
|
|
{
|
|
float u = 1.0f - t;
|
|
float t2 = t * t;
|
|
float t3 = t * t * t;
|
|
float u2 = u * u;
|
|
float u3 = u * u * u;
|
|
result = ( u3 * points[ 0 ] ) + ( 3.0f * u2 * t * points[ 1 ] ) + ( 3.0f * u * t2 * points[ 2 ] ) + ( t3 * points[ 3 ] );
|
|
}
|
|
};
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Evaluate the bezier curve of the specified order given a set of control
|
|
// points for the curve. Uses the BezierEvaluateImpl in order to allow
|
|
// template partial specialization for specific order curves.
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE, int ORDER >
|
|
void BezierEvaluate( const POINT_TYPE *pControlPoints, float t, POINT_TYPE &result )
|
|
{
|
|
BezierEvaluateImpl< POINT_TYPE, ORDER >::BezierEvaluate( pControlPoints, t, result );
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Generic order N Bezier curve tangent evaluation. Evaluates the derivative of
|
|
// the Bezier curve at the specified 0 to 1 parameter and returns the result.
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE, int ORDER >
|
|
struct BezierTangentImpl
|
|
{
|
|
static void BezierTangent( const POINT_TYPE *pControlPoints, float t, POINT_TYPE &result )
|
|
{
|
|
POINT_TYPE derPoints[ ORDER ];
|
|
for ( int i = 0; i < ORDER; ++i )
|
|
{
|
|
derPoints[ i ] = ( pControlPoints[ i + 1 ] - pControlPoints[ i ] ) * (float)ORDER;
|
|
}
|
|
BezierEvaluate< POINT_TYPE, ORDER - 1 >( derPoints, t, result );
|
|
}
|
|
};
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Partial specialization for linear Bezier curve tangent evaluation
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE >
|
|
struct BezierTangentImpl< POINT_TYPE, 1 >
|
|
{
|
|
static void BezierTangent( const POINT_TYPE *pControlPoints, float t, POINT_TYPE &result )
|
|
{
|
|
POINT_TYPE derPoint;
|
|
result = ( pControlPoints[ 1 ] - pControlPoints[ 0 ] );
|
|
}
|
|
};
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Partial specialization for quadratic Bezier curve tangent evaluation
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE >
|
|
struct BezierTangentImpl< POINT_TYPE, 2 >
|
|
{
|
|
static void BezierTangent( const POINT_TYPE *pControlPoints, float t, POINT_TYPE &result )
|
|
{
|
|
POINT_TYPE a = pControlPoints[ 0 ] + ( -2.0f * pControlPoints[ 1 ] ) + pControlPoints[ 2 ];
|
|
POINT_TYPE b = ( -2.0f * pControlPoints[ 0 ] ) + ( 2.0f * pControlPoints[ 1 ] );
|
|
result = ( 2.0f * a * t ) + b;
|
|
}
|
|
};
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Partial specialization for cubic Bezier curve tangent evaluation
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE >
|
|
struct BezierTangentImpl< POINT_TYPE, 3 >
|
|
{
|
|
static void BezierTangent( const POINT_TYPE *pControlPoints, float t, POINT_TYPE &result )
|
|
{
|
|
POINT_TYPE a = (-1.0f * pControlPoints[ 0 ] ) + ( 3.0f * pControlPoints[ 1 ] ) + (-3.0f * pControlPoints[ 2 ] ) + pControlPoints[ 3 ];
|
|
POINT_TYPE b = ( 3.0f * pControlPoints[ 0 ] ) + (-6.0f * pControlPoints[ 1 ] ) + ( 3.0f * pControlPoints[ 2 ] );
|
|
POINT_TYPE c = (-3.0f * pControlPoints[ 0 ] ) + ( 3.0f * pControlPoints[ 1 ] );
|
|
result = ( 3.0f * a * t * t ) + ( 2.0f * b * t ) + c;
|
|
}
|
|
};
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Evaluate the derivative of the bezier curve in order to compute the tangent
|
|
// of the curve the the specified parameter. Uses BezierTangentImpl in order
|
|
// to allow template partial specialization for specific order curves.
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE, int ORDER >
|
|
void BezierTangent( const POINT_TYPE *pControlPoints, float t, POINT_TYPE &result )
|
|
{
|
|
BezierTangentImpl< POINT_TYPE, ORDER >::BezierTangent( pControlPoints, t, result );
|
|
}
|
|
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// The CBezierCurve represents an a order N bezier curve defined by control
|
|
// points of an arbitrary dimension. The class has template parameters for both
|
|
// the order ( ORDER ) and the point type ( POINT_TYPE ). In general the point
|
|
// type is expected to be vector, vector2d, or vector4d, but may work with other
|
|
// types if the appropriate operators are provided.
|
|
//
|
|
//-----------------------------------------------------------------------------
|
|
template < class POINT_TYPE, int ORDER >
|
|
class CBezierCurve
|
|
{
|
|
protected:
|
|
|
|
static const int NUM_POINTS = ORDER + 1;
|
|
|
|
public:
|
|
|
|
// Default constructor, performs no initialization
|
|
CBezierCurve() {}
|
|
|
|
// Copy constructor
|
|
CBezierCurve( const CBezierCurve &source );
|
|
|
|
// Array constructor, initialize the bezier from an array of control points
|
|
explicit CBezierCurve( const POINT_TYPE controlPoints[ NUM_POINTS ] );
|
|
|
|
// Set all of the control points of the curve
|
|
void SetControlPoints( const POINT_TYPE controlPoints[ NUM_POINTS ] );
|
|
|
|
// Evaluate the curve at specified 0 to 1 parameter, returning the point on the curve
|
|
void Evaluate( float flParam, POINT_TYPE &point ) const;
|
|
|
|
// Compute the tangent vector at the specified parameter of the curve
|
|
void ComputeTangent( float flParam, POINT_TYPE &tangent ) const;
|
|
|
|
// Get the specified control point
|
|
const POINT_TYPE &ControlPoint( int index ) const { return m_ControlPoints[ index ]; }
|
|
|
|
protected:
|
|
|
|
POINT_TYPE m_ControlPoints[ NUM_POINTS ];
|
|
};
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Copy constructor
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE, int ORDER >
|
|
CBezierCurve< POINT_TYPE, ORDER >::CBezierCurve( const CBezierCurve &source )
|
|
{
|
|
m_ControlPoints = source.m_ControlPoints;
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Array constructor, initialize the bezier from an array of control points
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE, int ORDER >
|
|
CBezierCurve< POINT_TYPE, ORDER >::CBezierCurve( const POINT_TYPE controlPoints[ NUM_POINTS ] )
|
|
{
|
|
SetControlPoints( controlPoints );
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Set all of the control points of the curve
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE, int ORDER >
|
|
void CBezierCurve< POINT_TYPE, ORDER >::SetControlPoints( const POINT_TYPE controlPoints[ NUM_POINTS ] )
|
|
{
|
|
for ( int i = 0; i < NUM_POINTS; ++i )
|
|
{
|
|
m_ControlPoints[ i ] = controlPoints[ i ];
|
|
}
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Evaluate the bezier curve of the specified order given a set of control
|
|
// points for the curve
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE, int ORDER >
|
|
void CBezierCurve< POINT_TYPE, ORDER >::Evaluate( float t, POINT_TYPE &result ) const
|
|
{
|
|
BezierEvaluate< POINT_TYPE, ORDER >( m_ControlPoints, t, result );
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Compute the tangent vector at the specified parameter of the curve
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE, int ORDER >
|
|
void CBezierCurve< POINT_TYPE, ORDER>::ComputeTangent( float t, POINT_TYPE &tangent ) const
|
|
{
|
|
BezierTangent< POINT_TYPE, ORDER >( m_ControlPoints, t, tangent );
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// The CCubicBezierCurve class represents a third order specialization of the
|
|
// generic CBezierCurve class and provided additional functionality which is
|
|
// implemented specifically for the cubic form of the bezier curve.
|
|
//-----------------------------------------------------------------------------
|
|
template < class POINT_TYPE >
|
|
class CCubicBezierCurve : public CBezierCurve< POINT_TYPE, 3 >
|
|
{
|
|
|
|
public:
|
|
|
|
// Default constructor, performs no initialization
|
|
CCubicBezierCurve() {}
|
|
|
|
// Array constructor, initialize the bezier from an array of control points
|
|
explicit CCubicBezierCurve( const POINT_TYPE controlPoints[ 4 ] );
|
|
|
|
// Compute the specified number of points along the curve
|
|
void ComputePoints( POINT_TYPE *pPoints, int numPoints ) const;
|
|
|
|
// Fit the curve to a set of data points
|
|
bool FitToPoints( const POINT_TYPE *pPoints, float *pParams, int nPoints, int nMaxSteps, float flMaxError );
|
|
|
|
private:
|
|
|
|
// Compute the bezier factor values used for evaluation that are independent of the parameter value
|
|
void ComputeFactors( POINT_TYPE &a, POINT_TYPE &b, POINT_TYPE &c, POINT_TYPE &d ) const;
|
|
|
|
// Perform a single step of the iterative point fitting process
|
|
bool FitToPointsStep( const POINT_TYPE *pPoints, float *pParams, int nPoints, bool bReparaterize );
|
|
|
|
// Calculate the control points of the curve that best fit the sample points with the specified parameters
|
|
void ComputeControlPointsForSamples( const POINT_TYPE *pPoints, float *pParams, int nNumPoints );
|
|
|
|
// Re-parameterize the provided set of points, finding parameter values which provide
|
|
// points on the curve closer to the sample points than the current parameter values.
|
|
void ReparameterizePoints( const POINT_TYPE *pPoints, float *pParams, int nNumPoints ) const;
|
|
|
|
// Compute the maximum squared distance between the specified points and the curve
|
|
float ComputeMaxError( const POINT_TYPE *pPoints, const float *pParams, int nNumPoints ) const;
|
|
|
|
// Compute the sum of the squared distance between the specified points and the curve
|
|
float ComputeErrorSum( const POINT_TYPE *pPoints, const float *pParams, int nNumPoints ) const;
|
|
|
|
};
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Array constructor, initialize the bezier from an array of control points
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE >
|
|
CCubicBezierCurve< POINT_TYPE >::CCubicBezierCurve( const POINT_TYPE controlPoints[ 4 ] )
|
|
{
|
|
this->m_ControlPoints[ 0 ] = controlPoints[ 0 ];
|
|
this->m_ControlPoints[ 1 ] = controlPoints[ 1 ];
|
|
this->m_ControlPoints[ 2 ] = controlPoints[ 2 ];
|
|
this->m_ControlPoints[ 3 ] = controlPoints[ 3 ];
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Compute the specified number of points along the curve
|
|
//-----------------------------------------------------------------------------
|
|
template< class POINT_TYPE >
|
|
void CCubicBezierCurve< POINT_TYPE >::ComputePoints( POINT_TYPE *pPoints, int numPoints ) const
|
|
{
|
|
// Must evaluate at least two points.
|
|
if ( numPoints <= 1 )
|
|
return;
|
|
|
|
// Calculate the parameter increment for each step
|
|
const float flStep = 1.0f / ( numPoints - 1 );
|
|
|
|
// Compute the basis values that can be re-used for all of the point calculations
|
|
POINT_TYPE a, b, c, d;
|
|
ComputeFactors( a, b, c, d );
|
|
|
|
float t = 0;
|
|
for ( int i = 0; i < numPoints; ++i )
|
|
{
|
|
pPoints[ i ] = ( a * t * t * t ) + ( b * t * t ) + ( c * t ) + d;
|
|
t = t + flStep;
|
|
}
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Fit the curve to a set of data points
|
|
//-----------------------------------------------------------------------------
|
|
template < class POINT_TYPE >
|
|
bool CCubicBezierCurve< POINT_TYPE >::FitToPoints( const POINT_TYPE *pPoints, float *pParams, int nPoints, int nMaxSteps, float flMaxError )
|
|
{
|
|
if ( ( pPoints == NULL ) || ( pParams == NULL ) || ( nPoints < 2 ) )
|
|
return false;
|
|
|
|
// Compute the max error distance, the provided max error is assumed
|
|
// to be a percentage of based on the length of the curve.
|
|
float flLengthSQ = pPoints[ 0 ].DistToSqr( pPoints[ nPoints - 1 ] );
|
|
|
|
// Do one initial step using chord length parameterization.
|
|
FitToPointsStep( pPoints, pParams, nPoints, false );
|
|
float flError = ComputeMaxError( pPoints, pParams, nPoints );
|
|
|
|
// Check to see if the error is reasonable enough to be solved by iteration.
|
|
float flMaxIterationError = flLengthSQ * 0.1f;
|
|
if ( flError > flMaxIterationError )
|
|
{
|
|
POINT_TYPE vSegment = pPoints[ nPoints - 1 ] - pPoints[ 0 ];
|
|
this->m_ControlPoints[ 0 ] = pPoints[ 0 ];
|
|
this->m_ControlPoints[ 1 ] = pPoints[ 0 ] + ( vSegment * ONE_THIRD );
|
|
this->m_ControlPoints[ 2 ] = pPoints[ 0 ] + ( vSegment * TWO_THIRDS );
|
|
this->m_ControlPoints[ 3 ] = pPoints[ nPoints - 1 ];
|
|
return false;
|
|
}
|
|
|
|
// Iteratively improve the solution by re-evaluating the parameter values to make them match
|
|
// the sample points more closely and then re-fitting the curve using the least squares method.
|
|
int iStep = 0;
|
|
float flMaxErrorDist = flLengthSQ * ( flMaxError * flMaxError );
|
|
|
|
while ( ( flError > flMaxErrorDist ) && ( iStep < nMaxSteps ) )
|
|
{
|
|
FitToPointsStep( pPoints, pParams, nPoints, true );
|
|
flError = ComputeMaxError( pPoints, pParams, nPoints );
|
|
++iStep;
|
|
}
|
|
|
|
return ( flError <= flMaxErrorDist );
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Compute the bezier factor values used for evaluation that are independent of
|
|
// the parameter value
|
|
//-----------------------------------------------------------------------------
|
|
template < class POINT_TYPE >
|
|
void CCubicBezierCurve< POINT_TYPE >::ComputeFactors( POINT_TYPE &a, POINT_TYPE &b, POINT_TYPE &c, POINT_TYPE &d ) const
|
|
{
|
|
const POINT_TYPE *pControlPoints = this->m_ControlPoints;
|
|
|
|
a = (-1.0f * pControlPoints[ 0 ] ) + ( 3.0f * pControlPoints[ 1 ] ) + (-3.0f * pControlPoints[ 2 ] ) + pControlPoints[ 3 ];
|
|
b = ( 3.0f * pControlPoints[ 0 ] ) + (-6.0f * pControlPoints[ 1 ] ) + ( 3.0f * pControlPoints[ 2 ] );
|
|
c = (-3.0f * pControlPoints[ 0 ] ) + ( 3.0f * pControlPoints[ 1 ] );
|
|
d = ( 1.0f * pControlPoints[ 0 ] );
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Perform a single step of the iterative point fitting process
|
|
//-----------------------------------------------------------------------------
|
|
template < class POINT_TYPE >
|
|
bool CCubicBezierCurve< POINT_TYPE >::FitToPointsStep( const POINT_TYPE *pPoints, float *pParams, int nNumPoints, bool bReparameterize )
|
|
{
|
|
POINT_TYPE *pControlPoints = this->m_ControlPoints;
|
|
|
|
if ( ( pPoints == NULL ) || ( pParams == NULL ) )
|
|
return false;
|
|
|
|
if ( nNumPoints < 2 )
|
|
return false;
|
|
|
|
if ( nNumPoints == 2)
|
|
{
|
|
pControlPoints[ 0 ] = pPoints[ 0 ];
|
|
pControlPoints[ 1 ] = pPoints[ 0 ] + ( pPoints[ 1 ] - pPoints[ 0 ] ) * ONE_THIRD;
|
|
pControlPoints[ 2 ] = pPoints[ 0 ] + ( pPoints[ 1 ] - pPoints[ 0 ] ) * TWO_THIRDS;
|
|
pControlPoints[ 3 ] = pPoints[ 1 ];
|
|
pParams[ 0 ] = 0.0f;
|
|
pParams[ 1 ] = 1.0f;
|
|
return true;
|
|
}
|
|
|
|
if ( bReparameterize )
|
|
{
|
|
ReparameterizePoints( pPoints, pParams, nNumPoints );
|
|
}
|
|
else
|
|
{
|
|
// Chord length parameterization
|
|
float length = 0;
|
|
pParams[ 0 ] = 0;
|
|
for ( int i = 1; i < nNumPoints; ++i )
|
|
{
|
|
float distance = pPoints[ i - 1 ].DistTo( pPoints[ i ] );
|
|
length += distance;
|
|
pParams[ i ] = length;
|
|
}
|
|
for ( int i = 0; i < nNumPoints; ++i )
|
|
{
|
|
pParams[ i ] = pParams[ i ] / length;
|
|
}
|
|
}
|
|
|
|
ComputeControlPointsForSamples( pPoints, pParams, nNumPoints );
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Calculate the control points of the curve that best fit the sample points
|
|
// with the specified parameters.
|
|
//-----------------------------------------------------------------------------
|
|
template < class POINT_TYPE >
|
|
void CCubicBezierCurve< POINT_TYPE >::ComputeControlPointsForSamples( const POINT_TYPE *pPoints, float *pParams, int nPoints )
|
|
{
|
|
POINT_TYPE *pControlPoints = this->m_ControlPoints;
|
|
|
|
// Set end control points to the first and last sample points
|
|
pControlPoints[ 0 ] = pPoints[ 0 ];
|
|
pControlPoints[ 3 ] = pPoints[ nPoints - 1 ];
|
|
|
|
// Use the least squares method to calculate new controls points
|
|
float a1 = 0;
|
|
float a2 = 0;
|
|
float a12 = 0;
|
|
POINT_TYPE c1;
|
|
POINT_TYPE c2;
|
|
|
|
c1.Init();
|
|
c2.Init();
|
|
|
|
Assert( c1.IsZero() );
|
|
Assert( c2.IsZero() );
|
|
|
|
for ( int i = 0; i < nPoints; ++i )
|
|
{
|
|
const POINT_TYPE &p = pPoints[ i ];
|
|
|
|
float t = pParams[ i ];
|
|
float t2 = t * t;
|
|
float t3 = t * t2;
|
|
float t4 = t * t3;
|
|
float u = 1 - t;
|
|
float u2 = u * u;
|
|
float u3 = u * u2;
|
|
float u4 = u * u3;
|
|
|
|
a1 += t2 * u4;
|
|
a2 += t4 * u2;
|
|
a12 += t3 * u3;
|
|
|
|
POINT_TYPE vP = p - (u3 * pControlPoints[ 0 ]) - ( t3 * pControlPoints[ 3 ] );
|
|
c1 += ( ( 3 * t * u2 ) * vP );
|
|
c2 += ( ( 3 * t2 * u ) * vP );
|
|
}
|
|
|
|
a1 = 9.0f * a1;
|
|
a2 = 9.0f * a2;
|
|
a12 = 9.0f * a12;
|
|
|
|
const float flFactorTolerance = 0.000001f;
|
|
float flFactor = ( a1 * a2 - a12 * a12 );
|
|
if ( fabs( flFactor ) < flFactorTolerance )
|
|
{
|
|
POINT_TYPE vSegment = pControlPoints[ 3 ] - pControlPoints[ 0 ];
|
|
pControlPoints[ 1 ] = pControlPoints[ 0 ] + vSegment * ONE_THIRD;
|
|
pControlPoints[ 2 ] = pControlPoints[ 0 ] + vSegment * TWO_THIRDS;
|
|
}
|
|
else
|
|
{
|
|
pControlPoints[ 1 ] = ( a2 * c1 - a12 * c2 ) / flFactor;
|
|
pControlPoints[ 2 ] = ( a1 * c2 - a12 * c1 ) / flFactor;
|
|
}
|
|
|
|
// The subsequent re-parameterization relies on NewtonRaphson root finding which will
|
|
// fail if the tangents have an x delta of 0 or less, so ensure this does not happen.
|
|
float flMinStep = ( pControlPoints[ 3 ].x - pControlPoints[ 0 ].x ) * 0.0001f;
|
|
float flMinX = pControlPoints[ 0 ].x + flMinStep;
|
|
float flMaxX = pControlPoints[ 3 ].x - flMinStep;
|
|
pControlPoints[ 1 ].x = MAX( flMinX, MIN( flMaxX, pControlPoints[ 1 ].x ) );
|
|
pControlPoints[ 2 ].x = MAX( flMinX, MIN( flMaxX, pControlPoints[ 2 ].x ) );
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Find a better parameter for the specified point using the NewtonRaphson
|
|
// method, or with simple iteration if the NewtonRaphson method fails.
|
|
//-----------------------------------------------------------------------------
|
|
template < class POINT_TYPE >
|
|
void CCubicBezierCurve< POINT_TYPE >::ReparameterizePoints( const POINT_TYPE *pPoints, float *pParams, int nNumPoints ) const
|
|
{
|
|
const POINT_TYPE *pControlPoints = this->m_ControlPoints;
|
|
|
|
const float flTolerance = 0.0001f;
|
|
|
|
POINT_TYPE der1[ 3 ];
|
|
der1[ 0 ] = ( pControlPoints[ 1 ] - pControlPoints[ 0 ] ) * 3.0f;
|
|
der1[ 1 ] = ( pControlPoints[ 2 ] - pControlPoints[ 1 ] ) * 3.0f;
|
|
der1[ 2 ] = ( pControlPoints[ 3 ] - pControlPoints[ 2 ] ) * 3.0f;
|
|
|
|
POINT_TYPE der2[ 2 ];
|
|
der2[ 0 ] = ( der1[ 1 ] - der1[ 0 ] ) * 2.0f;
|
|
der2[ 1 ] = ( der1[ 2 ] - der1[ 1 ] ) * 2.0f;
|
|
|
|
// Compute the basis values that can be re-used for all of the point calculations
|
|
POINT_TYPE b3a, b3b, b3c, b3d;
|
|
ComputeFactors( b3a, b3b, b3c, b3d );
|
|
|
|
POINT_TYPE b2a = ( 1.0f * der1[ 0 ] ) + ( -2.0f * der1[ 1 ] ) + ( 1.0f * der1[ 2 ] );
|
|
POINT_TYPE b2b = ( -2.0f * der1[ 0 ] ) + ( 2.0f * der1[ 1 ] );
|
|
POINT_TYPE b2c = ( 1.0f * der1[ 0 ] );
|
|
|
|
POINT_TYPE b1a = der2[ 1 ] - der2[ 0 ];
|
|
POINT_TYPE b1b = der2[ 0 ];
|
|
|
|
float flPrevParam = 0;
|
|
for ( int iPoint = 0; iPoint < nNumPoints; ++iPoint )
|
|
{
|
|
float t = pParams[ iPoint ];
|
|
const POINT_TYPE &point = pPoints[ iPoint ];
|
|
|
|
POINT_TYPE curvePoint = ( b3a * t * t * t ) + ( b3b * t * t ) + ( b3c * t ) + b3d;
|
|
POINT_TYPE der1Point = ( b2a * t * t ) + ( b2b * t ) + b2c;
|
|
POINT_TYPE der2Point = ( b1a * t ) + b1b;
|
|
|
|
// Attempt to find a better parameter for the point
|
|
// using the NewtonRaphson root finding method.
|
|
POINT_TYPE vDelta = curvePoint - point;
|
|
float flNumerator = vDelta.Dot( der1Point );
|
|
float flDenominator = vDelta.Dot( der2Point ) + der1Point.Dot( der1Point );
|
|
float flRootParam = ( flDenominator == 0.0f ) ? t : t - ( flNumerator / flDenominator );
|
|
|
|
// We are not interested in any solutions outside the 0 to 1 range, so
|
|
// clamp the result. This may give a result that is farther than the
|
|
// original parameter, in which case the original parameter will be used.
|
|
flRootParam = MAX( 0.0f, MIN( 1.0f, flRootParam ) );
|
|
|
|
// Evaluate the parameter returned by the root finding, to
|
|
// determine if it is actually better parameter for the point.
|
|
float rp = flRootParam;
|
|
POINT_TYPE rootCurvePoint = ( b3a * rp * rp * rp ) + ( b3b * rp * rp ) + ( b3c * rp ) + b3d;
|
|
float flDist = point.DistToSqr( curvePoint );
|
|
float flDistRoot = point.DistToSqr( rootCurvePoint );
|
|
|
|
// If the parameter returned by the root finding method gives a point on the
|
|
// curve that is closer to the sample point than the current parameter make
|
|
// the new parameter the value found by the root finding method.
|
|
float flNewParam = t;
|
|
if ( flDistRoot <= flDist )
|
|
{
|
|
flNewParam = flRootParam;
|
|
}
|
|
else if ( flDist > flTolerance )
|
|
{
|
|
// If the root finding method failed, try to find a better parameter iteratively. This is
|
|
// basically a brute force method, but with a couple of observations which actually make it
|
|
// reasonable. First the direction to iterate from the current parameter can be deduced
|
|
// from the dot product of the vector from the point and the tangent of the curve. Second
|
|
// the range of iteration can be restricted such that values before the last parameter
|
|
// are not considered.
|
|
POINT_TYPE stepPoint;
|
|
float flStepParam = t;
|
|
float flStepDist = 0;
|
|
float flBestStepParam = t;
|
|
float flBestStepDist = flDist;
|
|
|
|
const int nMaxSteps = 10;
|
|
const float flBaseStepSize = MAX( t - flPrevParam, 0.001f ) / ( float )nMaxSteps;
|
|
float flStepSize = 0;
|
|
int nStep = 0;
|
|
|
|
// The numerator of the root finding method is the dot product between the vector from the
|
|
// sample point to the point on the curve and the tangent of the curve. The tangent of the
|
|
// curve tells us which way the curve is going and we want to move the parameter along the
|
|
// curve in the way which is moving closer to the point, so if the dot product of the
|
|
// tangent and the vector from the point on the curve to the sample point is positive then
|
|
// moving in a positive direction along the curve will bring us closer to the sample point.
|
|
// However, the numerator value used the vector from the sample point to the curve point,
|
|
// so negative value implies a positive movement.
|
|
if ( flNumerator < 0 )
|
|
{
|
|
flStepSize = flBaseStepSize;
|
|
}
|
|
else
|
|
{
|
|
flStepSize = -flBaseStepSize;
|
|
}
|
|
|
|
// Starting with the current parameter, move the parameter by the calculated step interval
|
|
// and evaluate the result. Continue as long as the result is closer than the previous
|
|
// best result and the specified maximum number of steps has not been reached.
|
|
while ( nStep < nMaxSteps )
|
|
{
|
|
flStepParam = MAX( 0.0f, MIN( 1.0f, flStepParam + flStepSize ) );
|
|
|
|
float sp = flStepParam;
|
|
stepPoint = ( b3a * sp * sp * sp ) + ( b3b * sp * sp ) + ( b3c * sp ) + b3d;
|
|
flStepDist = point.DistToSqr( stepPoint );
|
|
|
|
if ( flStepDist >= flBestStepDist )
|
|
break;
|
|
|
|
flBestStepParam = flStepParam;
|
|
flBestStepDist = flStepDist;
|
|
++nStep;
|
|
}
|
|
|
|
flNewParam = flBestStepParam;
|
|
}
|
|
|
|
// Update the parameter to the new value which provides
|
|
// a closer point on the curve to the sample point.
|
|
Assert( flNewParam >= 0.0f );
|
|
Assert( flNewParam <= 1.0f );
|
|
pParams[ iPoint ] = flNewParam;
|
|
|
|
// Save the old parameter so it may be used by the next point
|
|
// to determine the iteration range if the root finding fails
|
|
flPrevParam = t;
|
|
}
|
|
}
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Compute the maximum squared distance between the specified points and the
|
|
// curve
|
|
//-----------------------------------------------------------------------------
|
|
template < class POINT_TYPE >
|
|
float CCubicBezierCurve< POINT_TYPE >::ComputeMaxError( const POINT_TYPE *pPoints, const float *pParams, int nNumPoints ) const
|
|
{
|
|
float flMaxError = 0.0f;
|
|
|
|
POINT_TYPE a, b, c, d;
|
|
ComputeFactors( a, b, c, d );
|
|
|
|
for ( int iPoint = 0; iPoint < nNumPoints; ++iPoint )
|
|
{
|
|
const POINT_TYPE &samplePoint = pPoints[ iPoint ];
|
|
float t = pParams[ iPoint ];
|
|
|
|
POINT_TYPE curvePoint = ( a * t * t * t ) + ( b * t * t ) + ( c * t ) + d;
|
|
|
|
float flDistSQ = samplePoint.DistToSqr( curvePoint );
|
|
flMaxError = MAX( flDistSQ, flMaxError );
|
|
}
|
|
|
|
return flMaxError;
|
|
}
|
|
|
|
|
|
|
|
//-----------------------------------------------------------------------------
|
|
// Compute the sum of the squared distance between the specified points and the
|
|
// curve
|
|
//-----------------------------------------------------------------------------
|
|
template < class POINT_TYPE >
|
|
float CCubicBezierCurve< POINT_TYPE >::ComputeErrorSum( const POINT_TYPE *pPoints, const float *pParams, int nNumPoints ) const
|
|
{
|
|
float flErrorSum = 0.0f;
|
|
|
|
POINT_TYPE a, b, c, d;
|
|
ComputeFactors( a, b, c, d );
|
|
|
|
for ( int iPoint = 0; iPoint < nNumPoints; ++iPoint )
|
|
{
|
|
const POINT_TYPE &samplePoint = pPoints[ iPoint ];
|
|
float t = pParams[ iPoint ];
|
|
|
|
POINT_TYPE curvePoint = ( a * t * t * t ) + ( b * t * t ) + ( c * t ) + d;
|
|
|
|
float flDistSQ = samplePoint.DistToSqr( curvePoint );
|
|
flErrorSum += flDistSQ;
|
|
}
|
|
|
|
return flErrorSum;
|
|
}
|
|
|
|
|
|
#endif
|