Upgrade to Newton 3.14 and make it a thirdparty lib
This commit is contained in:
Jérôme Leclercq
641
thirdparty/src/newton/dContainers/dBezierSpline.cpp
vendored
Normal file
641
thirdparty/src/newton/dContainers/dBezierSpline.cpp
vendored
Normal file
@@ -0,0 +1,641 @@
|
||||
/* 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;
|
||||
}
|
||||
Reference in New Issue
Block a user