642 lines
17 KiB
C++
642 lines
17 KiB
C++
/* Copyright (c) <2003-2019> <Newton Game Dynamics>
|
|
*
|
|
* This software is provided 'as-is', without any express or implied
|
|
* warranty. In no event will the authors be held liable for any damages
|
|
* arising from the use of this software.
|
|
*
|
|
* Permission is granted to anyone to use this software for any purpose,
|
|
* including commercial applications, and to alter it and redistribute it
|
|
* freely
|
|
*/
|
|
|
|
#include "dContainersStdAfx.h"
|
|
#include "dBezierSpline.h"
|
|
|
|
dBezierSpline::dBezierSpline ()
|
|
:dContainersAlloc()
|
|
,m_knotVector(16)
|
|
,m_controlPoints(16)
|
|
,m_degree(0)
|
|
,m_knotsCount(0)
|
|
,m_controlPointsCount(0)
|
|
{
|
|
}
|
|
|
|
dBezierSpline::dBezierSpline (const dBezierSpline& src)
|
|
:m_knotVector(src.m_knotsCount)
|
|
,m_controlPoints(src.m_controlPointsCount)
|
|
,m_degree(0)
|
|
,m_knotsCount(0)
|
|
,m_controlPointsCount(0)
|
|
{
|
|
if (src.m_knotsCount) {
|
|
CreateFromKnotVectorAndControlPoints (src.m_degree, src.m_knotsCount - 2 * src.m_degree, &src.m_knotVector[src.m_degree], &src.m_controlPoints[0]);
|
|
}
|
|
}
|
|
|
|
dBezierSpline::~dBezierSpline ()
|
|
{
|
|
Clear();
|
|
}
|
|
|
|
void dBezierSpline::Clear()
|
|
{
|
|
m_degree = 0;
|
|
m_knotsCount = 0;
|
|
m_controlPointsCount = 0;
|
|
}
|
|
|
|
dBezierSpline& dBezierSpline::operator = (const dBezierSpline ©)
|
|
{
|
|
Clear();
|
|
if (copy.m_knotsCount) {
|
|
CreateFromKnotVectorAndControlPoints (copy.m_degree, copy.m_knotsCount - 2 * copy.m_degree, ©.m_knotVector[copy.m_degree], ©.m_controlPoints[0]);
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
int dBezierSpline::GetDegree () const
|
|
{
|
|
return m_degree;
|
|
}
|
|
|
|
void dBezierSpline::CreateFromKnotVectorAndControlPoints (int degree, int knotCount, const dFloat64* const knotVector, const dBigVector* const controlPoints)
|
|
{
|
|
Clear();
|
|
dAssert (knotCount);
|
|
dAssert (knotVector[0] == 0.0f);
|
|
dAssert (knotVector[knotCount - 1] == 1.0f);
|
|
|
|
m_degree = degree;
|
|
m_knotsCount = knotCount + 2 * degree;
|
|
m_controlPointsCount = knotCount + m_degree - 1;
|
|
|
|
for (int i = 0; i < m_controlPointsCount; i++) {
|
|
m_controlPoints[i] = controlPoints[i];
|
|
}
|
|
|
|
for (int i = 0; i < m_degree; i ++) {
|
|
m_knotVector[i] = dFloat64(0.0f);
|
|
m_knotVector[i + m_knotsCount - m_degree] = dFloat64(1.0f);
|
|
}
|
|
|
|
for (int i = 0; i < knotCount; i ++) {
|
|
m_knotVector[i + m_degree] = knotVector[i];
|
|
dAssert (m_knotVector[i + m_degree] >= m_knotVector[i + m_degree - 1]);
|
|
}
|
|
}
|
|
|
|
int dBezierSpline::GetKnotCount() const
|
|
{
|
|
return m_knotsCount;
|
|
}
|
|
|
|
dArray<dFloat64>& dBezierSpline::GetKnotArray()
|
|
{
|
|
return m_knotVector;
|
|
}
|
|
|
|
const dArray<dFloat64>& dBezierSpline::GetKnotArray() const
|
|
{
|
|
return m_knotVector;
|
|
}
|
|
|
|
dFloat64 dBezierSpline::GetKnot(int i) const
|
|
{
|
|
return m_knotVector[i];
|
|
}
|
|
|
|
int dBezierSpline::GetControlPointCount() const
|
|
{
|
|
return m_controlPointsCount;
|
|
}
|
|
|
|
dBigVector dBezierSpline::GetControlPoint(int i) const
|
|
{
|
|
return m_controlPoints[i];
|
|
}
|
|
|
|
void dBezierSpline::SetControlPoint(int i, const dBigVector& point)
|
|
{
|
|
m_controlPoints[i] = point;
|
|
}
|
|
|
|
dArray<dBigVector>& dBezierSpline::GetControlPointArray()
|
|
{
|
|
return m_controlPoints;
|
|
}
|
|
|
|
const dArray<dBigVector>& dBezierSpline::GetControlPointArray() const
|
|
{
|
|
return m_controlPoints;
|
|
}
|
|
|
|
int dBezierSpline::GetSpan(dFloat64 u) const
|
|
{
|
|
int low = m_degree;
|
|
int high = m_knotsCount - m_degree - 1;
|
|
|
|
dAssert (u >= 0.0f);
|
|
dAssert (u <= 1.0f);
|
|
while ((high - low) >= 4) {
|
|
int mid = (low + high) >> 1;
|
|
if (u > m_knotVector[mid]) {
|
|
low = mid;
|
|
} else {
|
|
high = mid;
|
|
}
|
|
}
|
|
|
|
dAssert (m_knotVector[low] <= u);
|
|
for (int i = low; i < m_degree + m_knotsCount + 1; i ++) {
|
|
if (m_knotVector[i + 1] >= u) {
|
|
return i;
|
|
}
|
|
}
|
|
dAssert (0);
|
|
return 0;
|
|
}
|
|
|
|
void dBezierSpline::BasicsFunctions (dFloat64 u, int span, dFloat64* const BasicFunctionsOut) const
|
|
{
|
|
BasicFunctionsOut[0] = 1.0f;
|
|
|
|
dFloat64* const left = dAlloca(dFloat64, m_knotsCount + 32);
|
|
dFloat64* const right = dAlloca(dFloat64, m_knotsCount + 32);
|
|
|
|
for (int j = 1; j <= m_degree; j ++) {
|
|
left[j] = u - m_knotVector[span + 1 - j];
|
|
right[j] = m_knotVector[span + j] - u;
|
|
|
|
dFloat64 saved = 0.0f;
|
|
for (int r = 0; r < j; r ++) {
|
|
dFloat64 temp = BasicFunctionsOut[r] / (right[r + 1] + left[j - r]);
|
|
BasicFunctionsOut[r] = saved + temp * right[r + 1];
|
|
saved = temp * left[j - r];
|
|
}
|
|
BasicFunctionsOut[j] = saved;
|
|
}
|
|
}
|
|
|
|
void dBezierSpline::BasicsFunctionsDerivatives (dFloat64 u, int span, dFloat64* const derivativesOut) const
|
|
{
|
|
dFloat64* const a = dAlloca(dFloat64, m_knotsCount + 32);
|
|
dFloat64* const ndu = dAlloca(dFloat64, m_knotsCount + 32);
|
|
dFloat64* const left = dAlloca(dFloat64, m_knotsCount + 32);
|
|
dFloat64* const right = dAlloca(dFloat64, m_knotsCount + 32);
|
|
|
|
const int width = m_degree + 1;
|
|
ndu[0] = 1.0f;
|
|
for (int j = 1; j <= m_degree; j ++) {
|
|
left[j] = u - m_knotVector[span + 1 - j];
|
|
right[j] = m_knotVector[span + j] - u;
|
|
dFloat64 saved = 0.0f;
|
|
for (int r = 0; r < j; r ++) {
|
|
ndu[j * width + r] = right[r + 1] + left[j - r];
|
|
|
|
dFloat64 temp = ndu[r * width + j - 1] / ndu[j * width + r];
|
|
ndu[r * width + j] = saved + temp * right[r + 1];
|
|
saved = temp * left[j - r];
|
|
}
|
|
ndu[j * width + j] = saved;
|
|
}
|
|
|
|
|
|
for (int j = 0; j <= m_degree; j ++) {
|
|
derivativesOut[width * 0 + j] = ndu [width * j + m_degree];
|
|
}
|
|
|
|
for (int r = 0; r <= m_degree; r ++) {
|
|
int s1 = 0;
|
|
int s2 = 1;
|
|
a[0] = 1.0f;
|
|
for (int k = 1; k <= m_degree; k ++) {
|
|
dFloat64 d = 0.0f;
|
|
int rk = r - k;
|
|
int pk = m_degree - k;
|
|
if (r >= k) {
|
|
a[width * s2 + 0] = a[width * s1 + 0] / ndu[width * (pk + 1) + rk];
|
|
d = a[width * s2 + 0] * ndu[width * rk + pk];
|
|
}
|
|
int j1 = 0;
|
|
int j2 = 0;
|
|
if (rk >= -1) {
|
|
j1 = 1;
|
|
} else {
|
|
j1 = -rk;
|
|
}
|
|
|
|
if ((r - 1) <= pk) {
|
|
j2 = k-1;
|
|
} else {
|
|
j2 = m_degree-r;
|
|
}
|
|
for (int j = j1; j <= j2; j ++) {
|
|
a[width * s2 + j] = (a[width * s1 + j] - a[width * s1 + j - 1]) / ndu[width * (pk + 1) + rk + j];
|
|
d += a[width * s2 + j] * ndu[width * (rk + j) + pk];
|
|
}
|
|
if (r <= pk) {
|
|
a[width * s2 + k] = -a[width * s1 + k - 1] / ndu[width * (pk + 1) + r];
|
|
d += a[width * s2 + k] * ndu[width * r + pk];
|
|
}
|
|
derivativesOut[width * k + r] = d;
|
|
dSwap(s1, s2);
|
|
}
|
|
}
|
|
|
|
int s = m_degree;
|
|
for (int k = 1; k <= m_degree; k ++) {
|
|
for (int j = 0; j <= m_degree; j ++) {
|
|
derivativesOut[width * k + j] *= s;
|
|
}
|
|
s *= (m_degree - k);
|
|
}
|
|
}
|
|
|
|
dBigVector dBezierSpline::CurvePoint (dFloat64 u, int span) const
|
|
{
|
|
dBigVector point (0.0f);
|
|
dFloat64* const basicFunctions = dAlloca(dFloat64, m_knotsCount + 32);
|
|
BasicsFunctions (u, span, basicFunctions);
|
|
for (int i = 0; i <= m_degree; i ++) {
|
|
point += m_controlPoints[span - m_degree + i].Scale (basicFunctions[i]);
|
|
}
|
|
return point;
|
|
}
|
|
|
|
dBigVector dBezierSpline::CurvePoint (dFloat64 u) const
|
|
{
|
|
u = dClamp (u, dFloat64 (0.0f), dFloat64 (1.0f));
|
|
int span = GetSpan(u);
|
|
return CurvePoint (u, span);
|
|
}
|
|
|
|
dBigVector dBezierSpline::CurveDerivative (dFloat64 u, int index) const
|
|
{
|
|
u = dClamp (u, dFloat64 (0.0f), dFloat64 (1.0f));
|
|
dAssert (index <= m_degree);
|
|
|
|
dFloat64* const basicsFuncDerivatives = dAlloca(dFloat64, m_knotsCount + 32);
|
|
int span = GetSpan(u);
|
|
BasicsFunctionsDerivatives (u, span, basicsFuncDerivatives);
|
|
|
|
const int with = m_degree + 1;
|
|
dBigVector point (0.0f);
|
|
for (int i = 0; i <= m_degree; i ++) {
|
|
point += m_controlPoints[span - m_degree + i].Scale (basicsFuncDerivatives[with * index + i]);
|
|
}
|
|
return point;
|
|
}
|
|
|
|
int dBezierSpline::CurveAllDerivatives (dFloat64 u, dBigVector* const derivatives) const
|
|
{
|
|
u = dMod (u, dFloat64(1.0f));
|
|
dFloat64* const basicsFuncDerivatives = dAlloca(dFloat64, m_knotsCount + 32);
|
|
int span = GetSpan(u);
|
|
BasicsFunctionsDerivatives (u, span, basicsFuncDerivatives);
|
|
|
|
const int with = m_degree + 1;
|
|
dBigVector point (0.0f);
|
|
for (int j = 0; j <= m_degree; j ++) {
|
|
dBigVector ck (0.0f);
|
|
for (int i = 0; i <= m_degree; i ++) {
|
|
ck += m_controlPoints[span - m_degree + i].Scale (basicsFuncDerivatives[with * j + i]);
|
|
}
|
|
derivatives[j] = ck;
|
|
}
|
|
|
|
return m_degree + 1;
|
|
}
|
|
|
|
void dBezierSpline::GlobalCubicInterpolation (int count, const dBigVector* const points, const dBigVector& firstTangent, const dBigVector& lastTangent)
|
|
{
|
|
CreateCubicKnotVector (count, points);
|
|
CreateCubicControlPoints (count, points, firstTangent, lastTangent);
|
|
}
|
|
|
|
void dBezierSpline::CreateCubicKnotVector(int count, const dBigVector* const points)
|
|
{
|
|
dFloat64 d = dFloat64(0.0f);
|
|
dAssert (count >= 2);
|
|
|
|
dFloat64* const u = dAlloca(dFloat64, m_knotsCount + 32);
|
|
u[0] = dFloat64(0.0f);
|
|
for (int i = 1; i < count; i ++) {
|
|
dBigVector step (points[i] - points[i - 1]);
|
|
dFloat64 len = dSqrt (step.DotProduct3(step));
|
|
u[i] = dSqrt (len);
|
|
d += u[i];
|
|
}
|
|
|
|
for (int i = 1; i < count; i ++) {
|
|
u[i] = u[i-1] + u[i] / d;
|
|
}
|
|
u[0] = dFloat64 (0.0f);
|
|
u[count - 1] = dFloat64(1.0f);
|
|
|
|
m_degree = 3;
|
|
m_knotsCount = count + 2 * m_degree;
|
|
|
|
for (int i = 0; i < (m_degree + 1); i ++) {
|
|
m_knotVector[i] = dFloat64(0.0f);
|
|
m_knotVector[i + m_knotsCount - m_degree - 1] = dFloat64(1.0f);
|
|
}
|
|
|
|
for (int i = 1; i < (count - 1); i ++) {
|
|
dFloat64 acc = dFloat64 (0.0f);
|
|
for (int j = 0; j < m_degree; j ++) {
|
|
acc += u[j + i - 1];
|
|
}
|
|
m_knotVector[m_degree + i] = acc / dFloat64 (3.0f);
|
|
}
|
|
}
|
|
|
|
void dBezierSpline::CreateCubicControlPoints(int count, const dBigVector* const points, const dBigVector& firstTangent, const dBigVector& lastTangent)
|
|
{
|
|
dFloat64 abc[4];
|
|
if ((m_knotsCount - 2 * (m_degree - 1)) != m_controlPointsCount) {
|
|
m_controlPointsCount = m_knotsCount - 2 * (m_degree - 1);
|
|
}
|
|
|
|
m_controlPoints[0] = points[0];
|
|
m_controlPoints[m_controlPointsCount - 1] = points[count - 1];
|
|
|
|
m_controlPoints[1] = m_controlPoints[0] + firstTangent.Scale (m_knotVector[m_degree + 1] / 3.0f);
|
|
m_controlPoints[m_controlPointsCount - 2] = m_controlPoints[m_controlPointsCount - 1] - lastTangent.Scale ((1.0f - m_knotVector[m_knotsCount - m_degree - 2]) / 3.0f);
|
|
if (count == 3) {
|
|
BasicsFunctions (m_knotVector[m_degree + 1], m_degree + 1, abc);
|
|
m_controlPoints[2] = points[1] - m_controlPoints[1].Scale (abc[0]) - m_controlPoints[3].Scale (abc[2]);
|
|
m_controlPoints[2] = m_controlPoints[2].Scale (1.0f / abc[1]);
|
|
} else {
|
|
dFloat64* const dd = dAlloca(dFloat64, m_knotsCount + 32);
|
|
BasicsFunctions (m_knotVector[m_degree + 1], m_degree + 1, abc);
|
|
dFloat64 den = abc[1];
|
|
m_controlPoints[2] = (points[1] - m_controlPoints[1].Scale (abc[0])).Scale (1.0f / den);
|
|
for (int i = 3; i < (count - 1); i ++) {
|
|
dd[i + 1] = abc[2] / den;
|
|
BasicsFunctions (m_knotVector[i + 2], i + 2, abc);
|
|
den = abc[1] - abc[0] * dd[i + 1];
|
|
m_controlPoints[i] = (points[i - 1] - m_controlPoints[i - 1].Scale (abc[0])).Scale (1.0f / den);
|
|
}
|
|
|
|
dd[count] = abc[2] / den;
|
|
BasicsFunctions (m_knotVector[count + 1], count + 1, abc);
|
|
den = abc[1] - abc[0] * dd[count];
|
|
m_controlPoints[count - 1] = (points[count - 2] - m_controlPoints[count].Scale (abc[2]) - m_controlPoints[count - 2].Scale (abc[0])).Scale (1.0f / den);
|
|
|
|
for (int i = count - 2; i >= 2; i --) {
|
|
m_controlPoints[i] -= m_controlPoints[i + 1].Scale (dd[i + 2]);
|
|
}
|
|
}
|
|
}
|
|
|
|
dFloat64 dBezierSpline::CalculateLength (dFloat64 tol) const
|
|
{
|
|
dBigVector stackPool[32][3];
|
|
int stack = 0;
|
|
|
|
dFloat64 length = 0.0f;
|
|
dFloat64 tol2 = tol * tol;
|
|
dFloat64 u0 = m_knotVector[m_degree];
|
|
dBigVector p0 (CurvePoint (u0));
|
|
|
|
for (int i = m_degree; i < (m_knotsCount - m_degree - 1); i ++) {
|
|
dFloat64 u1 = m_knotVector[i + 1];
|
|
dBigVector p1 (CurvePoint (u1));
|
|
stackPool[stack][0] = p0;
|
|
stackPool[stack][1] = p1;
|
|
stackPool[stack][2] = dBigVector (u0, u1, dFloat64(0.0f), dFloat64(0.0f));
|
|
stack ++;
|
|
while (stack) {
|
|
stack --;
|
|
dBigVector q0 (stackPool[stack][0]);
|
|
dBigVector q1 (stackPool[stack][1]);
|
|
dFloat64 t0 = stackPool[stack][2][0];
|
|
dFloat64 t1 = stackPool[stack][2][1];
|
|
dFloat64 t01 = (t1 + t0) * 0.5f;
|
|
|
|
dBigVector p01 ((q1 + q0).Scale (0.5f));
|
|
dBigVector q01 (CurvePoint (t01));
|
|
dBigVector err (q01 - p01);
|
|
|
|
dFloat64 err2 = err.DotProduct3(err);
|
|
if (err2 < tol2) {
|
|
dBigVector step (q1 - q0);
|
|
length += dSqrt (step.DotProduct3(step));
|
|
} else {
|
|
stackPool[stack][0] = q01;
|
|
stackPool[stack][1] = q1;
|
|
stackPool[stack][2] = dBigVector (t01, t1, dFloat64(0.0f), dFloat64(0.0f));
|
|
stack ++;
|
|
|
|
stackPool[stack][0] = q0;
|
|
stackPool[stack][1] = q01;
|
|
stackPool[stack][2] = dBigVector (t0, t01, dFloat64(0.0f), dFloat64(0.0f));
|
|
stack ++;
|
|
}
|
|
}
|
|
u0 = u1;
|
|
p0 = p1;
|
|
}
|
|
|
|
return length;
|
|
}
|
|
|
|
void dBezierSpline::InsertKnot (dFloat64 u)
|
|
{
|
|
const int k = GetSpan(u);
|
|
int multiplicity = 0;
|
|
for (int i = 0; i < m_degree; i ++) {
|
|
multiplicity += (dAbs (m_knotVector[k + i + 1] - u) < dFloat64 (1.0e-5f)) ? 1 : 0;
|
|
}
|
|
if (multiplicity == m_degree) {
|
|
return;
|
|
}
|
|
|
|
for (int i = m_knotsCount; i > (k + 1); i --) {
|
|
m_knotVector[i] = m_knotVector[i - 1];
|
|
}
|
|
m_knotVector[k + 1] = u;
|
|
|
|
dBigVector Rw[16];
|
|
for (int i = 0; i <= m_degree; i ++) {
|
|
Rw[i] = m_controlPoints[k - m_degree + i];
|
|
}
|
|
|
|
const int m = k - m_degree + 1;
|
|
dAssert(m >= 0);
|
|
dAssert((k + 1 - 1 - 0) >= 0);
|
|
dAssert((m_degree - 1 - 0) >= 0);
|
|
|
|
for (int i = 0; i <= (m_degree - 1); i ++) {
|
|
dFloat64 alpha = (u - m_knotVector[m + i]) / (m_knotVector[i + k + 1] - m_knotVector[m + i]);
|
|
Rw[i] = Rw[i + 1].Scale (alpha) + Rw[i].Scale (dFloat64 (1.0f) - alpha);
|
|
}
|
|
|
|
for (int i = m_controlPointsCount; i > k; i--) {
|
|
m_controlPoints[i] = m_controlPoints[i - 1];
|
|
}
|
|
m_controlPoints[m] = Rw[0];
|
|
m_controlPoints[k + 1 - 1 - 0] = Rw[m_degree - 1 - 0];
|
|
for (int i = m + 1; i < k; i++) {
|
|
dAssert((i - m) >= 0);
|
|
m_controlPoints[i] = Rw[i - m];
|
|
}
|
|
|
|
m_knotsCount ++;
|
|
m_controlPointsCount ++;
|
|
}
|
|
|
|
bool dBezierSpline::RemoveKnot (dFloat64 u, dFloat64 tol)
|
|
{
|
|
int r = GetSpan(u) + 1;
|
|
dAssert (m_knotVector[r - 1] < u);
|
|
if (dAbs (m_knotVector[r] - u) > 1.0e-5f) {
|
|
return false;
|
|
}
|
|
|
|
int s = 1;
|
|
int last = r - s;
|
|
int first = r - m_degree;
|
|
int ord = m_degree + 1;
|
|
dBigVector temp[16];
|
|
|
|
bool removableFlag = false;
|
|
int t = 0;
|
|
for ( ; t < m_degree; t ++) {
|
|
int off = first - 1;
|
|
temp[0] = m_controlPoints[off];
|
|
temp[last + 1 - off] = m_controlPoints[last + 1];
|
|
int i = first;
|
|
int j = last;
|
|
int ii = 1;
|
|
int jj = last - off;
|
|
|
|
while ((j - i) > t) {
|
|
dFloat64 alpha_i = (u - m_knotVector[i]) / (m_knotVector[i + ord + t] - m_knotVector[i]);
|
|
dFloat64 alpha_j = (u - m_knotVector[j - t]) / (m_knotVector[j + ord] - m_knotVector[j - t]);
|
|
temp[ii] = (m_controlPoints[i] - temp[ii - 1].Scale (dFloat64 (1.0f) - alpha_i)).Scale (dFloat64 (1.0f) / alpha_i);
|
|
temp[jj] = (m_controlPoints[j] - temp[jj + 1].Scale (alpha_j)).Scale (dFloat64 (1.0f) / (dFloat64 (1.0f) - alpha_j));
|
|
i ++;
|
|
j --;
|
|
ii ++;
|
|
jj --;
|
|
}
|
|
if ((j - i) < t) {
|
|
dBigVector diff (temp[ii - 1] - temp[jj + 1]);
|
|
removableFlag = diff.DotProduct3(diff) < (tol * tol);
|
|
} else {
|
|
dFloat64 alpha_i = (u - m_knotVector[i]) / (m_knotVector[i + ord + t] - m_knotVector[i]);
|
|
dBigVector p (temp[ii + t + 1].Scale (alpha_i) + temp[ii - 1].Scale (dFloat64 (1.0f) - alpha_i));
|
|
dBigVector diff (m_controlPoints[i] - p);
|
|
removableFlag = diff.DotProduct3(diff) < (tol * tol);
|
|
}
|
|
if (!removableFlag) {
|
|
break;
|
|
}
|
|
|
|
i = first;
|
|
j = last;
|
|
while ((j - 1) > t) {
|
|
m_controlPoints[i] = temp[i - off];
|
|
m_controlPoints[j] = temp[j - off];
|
|
i ++;
|
|
j --;
|
|
}
|
|
first --;
|
|
last ++;
|
|
}
|
|
|
|
if (t) {
|
|
for (int k = r + t; k < m_knotsCount; k ++) {
|
|
m_knotVector[k - t] = m_knotVector[k];
|
|
}
|
|
|
|
int fOut = (2 * r - s - m_degree) / 2;
|
|
int j = fOut;
|
|
int i = j;
|
|
for (int k = 1; k < t; k ++) {
|
|
if ((k % 2) == 1) {
|
|
i ++;
|
|
} else {
|
|
j = j - 1;
|
|
}
|
|
}
|
|
for (int k = i + 1; k < m_controlPointsCount; k ++) {
|
|
m_controlPoints[j] = m_controlPoints[k];
|
|
j ++;
|
|
}
|
|
|
|
m_knotsCount -= t;
|
|
m_controlPointsCount -= t;
|
|
}
|
|
|
|
|
|
return removableFlag;
|
|
}
|
|
|
|
dFloat64 dBezierSpline::FindClosestKnot(dBigVector& closestPoint, const dBigVector& point, int subdivitionSteps) const
|
|
{
|
|
int startSpan = m_degree;
|
|
dFloat64 bestU = 0.0f;
|
|
dFloat64 distance2 = 1.0e10f;
|
|
dBigVector closestControlPoint(m_controlPoints[0]);
|
|
subdivitionSteps = dMax(subdivitionSteps, 1);
|
|
dFloat64 scale = 1.0f / subdivitionSteps;
|
|
for (int span = m_degree; span < (m_knotsCount - m_degree - 1); span++) {
|
|
dFloat64 param = 0.0f;
|
|
for (int i = 0; i < subdivitionSteps; i++) {
|
|
dFloat64 u = m_knotVector[span] + (m_knotVector[span + 1] - m_knotVector[span]) * param;
|
|
param += scale;
|
|
dBigVector p(CurvePoint(u, span));
|
|
dBigVector dp(p - point);
|
|
dFloat64 dist2 = dp.DotProduct3(dp);
|
|
if (dist2 < distance2) {
|
|
bestU = u;
|
|
startSpan = span;
|
|
distance2 = dist2;
|
|
closestControlPoint = p;
|
|
}
|
|
}
|
|
}
|
|
|
|
dBigVector p(CurvePoint(0.999f));
|
|
dBigVector dp(p - point);
|
|
dFloat64 dist2 = dp.DotProduct3(dp);
|
|
if (dist2 < distance2) {
|
|
bestU = dFloat64(0.999f);
|
|
startSpan = m_knotsCount - m_degree - 2;
|
|
closestControlPoint = p;
|
|
}
|
|
|
|
dBigVector derivatives[32];
|
|
dFloat64 u0 = bestU;
|
|
|
|
bool stop = false;
|
|
for (int i = 0; (i < 20) && !stop; i++) {
|
|
CurveAllDerivatives(u0, derivatives);
|
|
|
|
dBigVector dist(closestControlPoint - point);
|
|
dFloat64 num = derivatives[1].DotProduct3(dist);
|
|
dFloat64 den = derivatives[2].DotProduct3(dist) + derivatives[1].DotProduct3(derivatives[1]);
|
|
dFloat64 u1 = dClamp(u0 - num / den, dFloat64(0.0), dFloat64(1.0));
|
|
if (u1 < m_knotVector[startSpan]) {
|
|
startSpan--;
|
|
dAssert(startSpan >= 0);
|
|
} else if (u1 >= m_knotVector[startSpan + 1]) {
|
|
startSpan++;
|
|
dAssert(startSpan < (m_knotsCount - m_degree));
|
|
}
|
|
|
|
dAssert(startSpan >= m_degree);
|
|
dAssert(startSpan <= (m_knotsCount - m_degree - 1));
|
|
closestControlPoint = CurvePoint(u1, startSpan);
|
|
|
|
stop |= (dAbs(u1 - u0) < 1.0e-10) || (num * num < ((dist.DotProduct3(dist)) * (derivatives[1].DotProduct3(derivatives[1])) * 1.0e-10));
|
|
u0 = u1;
|
|
}
|
|
|
|
closestPoint = closestControlPoint;
|
|
return u0;
|
|
}
|