双圆弧插值算法（三，代码实现）

双圆弧插值算法（三，代码实现）
交互式演示这是一个用HTML5编写的交互式演示。要移动控制点，请单击并拖动它们。若要移动切线，请单击并拖动控制点外的区域。默认情况下，曲线保持d1和d2相等，但也可以在下面指定自定义d1值。

代码
到目前为止，我们只讨论了二维情况。让我们编写一些C++代码来解决三维的情况。它非常相似，除了每个弧可以在不同的平面上对齐。这将在查找旋转方向和平面法线时创建一些调整。经过几次交叉积后，一切都成功了。这些代码示例是在以下许可下发布的。
/******************************************************************************
Copyright © 2014 Ryan Juckett
http://www.ryanjuckett.com/

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, subject to the following restrictions:

The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
This notice may not be removed or altered from any source
distribution.
/ Here’s our vector type. It’s about as basic as vector types come.//
//******************************************************************************
struct tVec3
{
float m_x;
float m_y;
float m_z;
}; Now, let’s define some math functions to help with common operations (mostly linear algebra).//******************************************************************************
// Compute the dot product of two vectors.
//******************************************************************************
float Vec_DotProduct(const tVec3 & lhs, const tVec3 & rhs)
{
return lhs.m_xrhs.m_x + lhs.m_yrhs.m_y + lhs.m_z*rhs.m_z;
}

//******************************************************************************
// Compute the cross product of two vectors.
//******************************************************************************
void Vec_CrossProduct(tVec3 * pResult, const tVec3 & lhs, const tVec3 & rhs)
{
float x = lhs.m_yrhs.m_z - lhs.m_zrhs.m_y;
float y = lhs.m_zrhs.m_x - lhs.m_xrhs.m_z;
float z = lhs.m_xrhs.m_y - lhs.m_yrhs.m_x;

pResult->m_x = x;
pResult->m_y = y;
pResult->m_z = z;
}

//******************************************************************************
// Compute the sum of two vectors.
//******************************************************************************
void Vec_Add(tVec3 * pResult, const tVec3 & lhs, const tVec3 & rhs)
{
pResult->m_x = lhs.m_x + rhs.m_x;
pResult->m_y = lhs.m_y + rhs.m_y;
pResult->m_z = lhs.m_z + rhs.m_z;
}

//******************************************************************************
// Compute the difference of two vectors.
//******************************************************************************
void Vec_Subtract(tVec3 * pResult, const tVec3 & lhs, const tVec3 & rhs)
{
pResult->m_x = lhs.m_x - rhs.m_x;
pResult->m_y = lhs.m_y - rhs.m_y;
pResult->m_z = lhs.m_z - rhs.m_z;
}

//******************************************************************************
// Compute a scaled vector.
//******************************************************************************
void Vec_Scale(tVec3 * pResult, const tVec3 & lhs, float rhs)
{
pResult->m_x = lhs.m_xrhs;
pResult->m_y = lhs.m_yrhs;
pResult->m_z = lhs.m_z*rhs;
}

//******************************************************************************
// Add a vector to a scaled vector.
//******************************************************************************
void Vec_AddScaled(tVec3 * pResult, const tVec3 & lhs, const tVec3 & rhs, float rhsScale)
{
pResult->m_x = lhs.m_x + rhs.m_xrhsScale;
pResult->m_y = lhs.m_y + rhs.m_yrhsScale;
pResult->m_z = lhs.m_z + rhs.m_z*rhsScale;
}

//******************************************************************************
// Compute the magnitude of a vector.
//******************************************************************************
float Vec_Magnitude(const tVec3 & lhs)
{
return sqrtf(Vec_DotProduct(lhs,lhs));
}

//******************************************************************************
// Check if the vector length is within epsilon of 1
//******************************************************************************
bool Vec_IsNormalized_Eps(const tVec3 & value, float epsilon)
{
const float sqrMag = Vec_DotProduct(value,value);
return sqrMag >= (1.0f - epsilon)(1.0f - epsilon)
&& sqrMag <= (1.0f + epsilon)(1.0f + epsilon);
}

//******************************************************************************
// Return 1 or -1 based on the sign of a real number.
//******************************************************************************
inline float Sign(float val)
{
return (val < 0.0f) ? -1.0f : 1.0f;
} The algorithm is broken up into two parts. First, we provide a function to compute the pair of arcs making up the curve (this basically covers all of the fun math I showed above). Second, we provide a function to interpolate across the biarc curve. This is the helper type representing a computed arc. It is the output of the first part (biarc computation) and the input to the second part (biarc interpolation). The set of data stored in this type has been chosen to reduce the number of operations in the interpolation process. This lets the user compute the biarc once, and then compute a bunch of points along it very fast. In our interpolation function, we will do a proper blend across each circular arc, but you might instead want to convert the biarc into something like an approximate Bézier curve such that it is compatible with other systems. In that case, you might not need all these values.//******************************************************************************
// Information about an arc used in biarc interpolation. Use
// Vec_BiarcInterp_ComputeArcs to compute the values and use Vec_BiarcInterp
// to interpolate along the arc pair.
//******************************************************************************
struct tBiarcInterp_Arc
{
tVec3 m_center; // center of the circle (or line)
tVec3 m_axis1; // vector from center to the end point
tVec3 m_axis2; // vector from center edge perpendicular to axis1
float m_radius; // radius of the circle (zero for lines)
float m_angle; // angle to rotate from axis1 towards axis2
float m_arcLen; // distance along the arc
}; This is a helper function for computing data about a single arc. This isn’t intended as a user facing function.//******************************************************************************
// Compute a single arc based on an end point and the vector from the endpoint
// to connection point.
//******************************************************************************
void BiarcInterp_ComputeArc
(
tVec3 * pCenter, // Out: Center of the circle or straight line.
float * pRadius, // Out: Zero for straight lines
float * pAngle, // Out: Angle of the arc
const tVec3 & point,
const tVec3 & tangent,
const tVec3 & pointToMid
)
{
// assume that the tangent is normalized
assert( Vec_IsNormalized_Eps(tangent,0.01f) );

const float c_Epsilon = 0.0001f;

// compute the normal to the arc plane
tVec3 normal;
Vec_CrossProduct(&normal, pointToMid, tangent);

// Compute an axis within the arc plane that is perpendicular to the tangent.
// This will be coliniear with the vector from the center to the end point.
tVec3 perpAxis;
Vec_CrossProduct(&perpAxis, tangent, normal);

const float denominator = 2.0f * Vec_DotProduct(perpAxis, pointToMid);

if (fabs(denominator) < c_Epsilon)
{
// The radius is infinite, so use a straight line. Place the center point in the
// middle of the line.
Vec_AddScaled(pCenter, point, pointToMid, 0.5f);
*pRadius = 0.0f;
*pAngle = 0.0f;
}
else
{
// Compute the distance to the center along perpAxis
const float centerDist = Vec_DotProduct(pointToMid,pointToMid) / denominator;
Vec_AddScaled(pCenter, point, perpAxis, centerDist);

// Compute the radius in absolute units
const float perpAxisMag = Vec_Magnitude(perpAxis);
const float radius = fabs(centerDist*perpAxisMag);

// Compute the arc angle
float angle;
if (radius < c_Epsilon)
{
angle = 0.0f;
}
else
{
const float invRadius = 1.0f / radius;

// Compute normalized directions from the center to the connection point
// and from the center to the end point.
tVec3 centerToMidDir;
tVec3 centerToEndDir;

Vec_Subtract(&centerToMidDir, point, *pCenter);
Vec_Scale(&centerToEndDir, centerToMidDir, invRadius);

Vec_Add(&centerToMidDir, centerToMidDir, pointToMid);
Vec_Scale(&centerToMidDir, centerToMidDir, invRadius);

// Compute the rotation direction
const float twist = Vec_DotProduct(perpAxis, pointToMid);

// Compute angle.
angle = acosf( Vec_DotProduct(centerToEndDir,centerToMidDir) ) * Sign(twist);
}

// output the radius and angle
pRadius = radius;
pAngle = angle;
}
}Here is the user facing function to generate a biarc from a pair of control points. It only supports the case where d1d1 and d2d2 are solved to be equal.//****************************************************************************
// Compute a pair of arcs to pass into Vec_BiarcInterp
// http://www.ryanjuckett.com/programming/biarc-interpolation/
//******************************************************************************
void BiarcInterp_ComputeArcs
(
tBiarcInterp_Arc * pArc1,
tBiarcInterp_Arc * pArc2,
const tVec3 & p1, // start position
const tVec3 & t1, // start tangent
const tVec3 & p2, // end position
const tVec3 & t2 // end tangent
)
{
assert( Vec_IsNormalized_Eps(t1,0.01f) );
assert( Vec_IsNormalized_Eps(t2,0.01f) );

const float c_Pi = 3.1415926535897932384626433832795f;
const float c_2Pi = 6.2831853071795864769252867665590f;
const float c_Epsilon = 0.0001f;

tVec3 v;
Vec_Subtract(&v, p2, p1);

const float vDotV = Vec_DotProduct(v,v);

// if the control points are equal, we don’t need to interpolate
if (vDotV < c_Epsilon)
{
pArc1->m_center = p1;
pArc2->m_radius = 0.0f;
pArc1->m_axis1 = v;
pArc1->m_axis2 = v;
pArc1->m_angle = 0.0f;
pArc1->m_arcLen = 0.0f;

pArc2->m_center = p1;
pArc2->m_radius = 0.0f;
pArc2->m_axis1 = v;
pArc2->m_axis2 = v;
pArc2->m_angle = 0.0f;
pArc2->m_arcLen = 0.0f;
return;
}

// computw the denominator for the quadratic formula
tVec3 t;
Vec_Add(&t, t1, t2);

const float vDotT = Vec_DotProduct(v,t);
const float t1DotT2 = Vec_DotProduct(t1,t2);
const float denominator = 2.0f*(1.0f - t1DotT2);

// if the quadratic formula denominator is zero, the tangents are equal and we need a special case
float d;
if (denominator < c_Epsilon)
{
const float vDotT2 = Vec_DotProduct(v,t2);

// if the special case d is infinity, the only solution is to interpolate across two semicircles
if ( fabs(vDotT2) < c_Epsilon )
{
const float vMag = sqrtf(vDotV);
const float invVMagSqr = 1.0f / vDotV;

// compute the normal to the plane containing the arcs
// (this has length vMag)
tVec3 planeNormal;
Vec_CrossProduct(&planeNormal, v, t2);

// compute the axis perpendicular to the tangent direction and aligned with the circles
// (this has length vMag*vMag)
tVec3 perpAxis;
Vec_CrossProduct(&perpAxis, planeNormal, v);

float radius= vMag * 0.25f;

tVec3 centerToP1;
Vec_Scale(&centerToP1, v, -0.25f);

// interpolate across two semicircles
Vec_Subtract(&pArc1->m_center, p1, centerToP1);
pArc1->m_radius= radius;
pArc1->m_axis1= centerToP1;
Vec_Scale(&pArc1->m_axis2, perpAxis, radius*invVMagSqr);
pArc1->m_angle= c_Pi;
pArc1->m_arcLen= c_Pi * radius;

Vec_Add(&pArc2->m_center, p2, centerToP1);
pArc2->m_radius= radius;
Vec_Scale(&pArc2->m_axis1, centerToP1, -1.0f);
Vec_Scale(&pArc2->m_axis2, perpAxis, -radius*invVMagSqr);
pArc2->m_angle= c_Pi;
pArc2->m_arcLen= c_Pi * radius;

return;
}
else
{
// compute distance value for equal tangents
d = vDotV / (4.0f * vDotT2);
}
}
else
{
// use the positive result of the quadratic formula
const float discriminant = vDotTvDotT + denominatorvDotV;
d = (-vDotT + sqrtf(discriminant)) / denominator;
}

// compute the connection point (i.e. the mid point)
tVec3 pm;
Vec_Subtract(&pm, t1, t2);
Vec_AddScaled(&pm, p2, pm, d);
Vec_Add(&pm, pm, p1);
Vec_Scale(&pm, pm, 0.5f);

// compute vectors from the end points to the mid point
tVec3 p1ToPm, p2ToPm;
Vec_Subtract(&p1ToPm, pm, p1);
Vec_Subtract(&p2ToPm, pm, p2);

// compute the arcs
tVec3 center1, center2;
float radius1, radius2;
float angle1, angle2;
BiarcInterp_ComputeArc( &center1, &radius1, &angle1, p1, t1, p1ToPm );
BiarcInterp_ComputeArc( &center2, &radius2, &angle2, p2, t2, p2ToPm );

// use the longer path around the circle if d is negative
if (d < 0.0f)
{
angle1= Sign(angle1)*c_2Pi - angle1;
angle2= Sign(angle2)*c_2Pi - angle2;
}

// output the arcs
// (the radius will be set to zero when the arc is a straight line)
pArc1->m_center = center1;
pArc1->m_radius = radius1;
Vec_Subtract(&pArc1->m_axis1, p1, center1); // redundant from Vec_BiarcInterp_ComputeArc
Vec_Scale(&pArc1->m_axis2, t1, radius1);
pArc1->m_angle = angle1;
pArc1->m_arcLen = (radius1 == 0.0f) ? Vec_Magnitude(p1ToPm) : fabs(radius1 * angle1);

pArc2->m_center = center2;
pArc2->m_radius = radius2;
Vec_Subtract(&pArc2->m_axis1, p2, center2); // redundant from Vec_BiarcInterp_ComputeArc
Vec_Scale(&pArc2->m_axis2, t2, -radius2);
pArc2->m_angle = angle2;
pArc2->m_arcLen = (radius2 == 0.0f) ? Vec_Magnitude(p2ToPm) : fabs(radius2 * angle2);
} Finally, we have the function to compute the fractional position along the biarc curve. Arc length is used to create a smooth distribution. Before interpolating, it is sometimes useful to inspect the computed arcs. For example, you might decide that a biarc with too much curvature will look bad and switch to linear interpolation. This could be checked by seeing if the arc lengths sum up to be greater than a semicircle placed at the control points (i.e. arcLen1+arcLen2>π2∣p2−p1∣arcLen1+arcLen2>π2∣p2−p1∣).//******************************************************************************
// Use a biarc to interpolate between two points such that the interpolation
// direction aligns with associated tangents.
// http://www.ryanjuckett.com/programming/biarc-interpolation/
//******************************************************************************
void BiarcInterp
(
tVec3 * pResult, // interpolated point
const tBiarcInterp_Arc & arc1,
const tBiarcInterp_Arc & arc2,
float frac // [0,1] fraction along the biarc
)
{
assert( frac >= 0.0f && frac <= 1.0f );

const float epsilon = 0.0001f;

// compute distance along biarc
const float totalDist = arc1.m_arcLen + arc2.m_arcLen;
const float fracDist = frac * totalDist;

// choose the arc to evaluate
if (fracDist < arc1.m_arcLen)
{
if (arc1.m_arcLen < epsilon)
{
// just output the end point
Vec_Add(pResult, arc1.m_center, arc1.m_axis1);
}
else
{
const float arcFrac = fracDist / arc1.m_arcLen;
if (arc1.m_radius == 0.0f)
{
// interpolate along the line
Vec_AddScaled(pResult, arc1.m_center, arc1.m_axis1, -arcFrac2.0f + 1.0f);
}
else
{
// interpolate along the arc
float angle = arc1.m_anglearcFrac;
float sinRot = sinf(angle);
float cosRot = cosf(angle);

Vec_AddScaled(pResult, arc1.m_center, arc1.m_axis1, cosRot);
Vec_AddScaled(pResult, pResult, arc1.m_axis2, sinRot);
}
}
}
else
{
if (arc2.m_arcLen < epsilon)
{
// just output the end point
Vec_Add(pResult, arc1.m_center, arc1.m_axis2);
}
else
{
const float arcFrac = (fracDist-arc1.m_arcLen) / arc2.m_arcLen;
if (arc2.m_radius == 0.0f)
{
// interpolate along the line
Vec_AddScaled(pResult, arc2.m_center, arc2.m_axis1, arcFrac2.0f - 1.0f);
}
else
{
// interpolate along the arc
float angle = arc2.m_angle*(1.0f-arcFrac);
float sinRot = sinf(angle);
float cosRot = cosf(angle);

Vec_AddScaled(pResult, arc2.m_center, arc2.m_axis1, cosRot);
Vec_AddScaled(pResult, *pResult, arc2.m_axis2, sinRot);
}
}
}
}

双圆弧插值算法（三，代码实现）相关推荐

双圆弧插值算法（二）
双圆弧插值算法(二) 找到中心找到连接点后,就可以求解圆心.我们定义一个向量,n1,垂直于t1.这最终是一个与(c1−p1)平行的标准化向量.从p1到c1的方向. 综合起来,我们得到了c1的解. 通 ...
双圆弧插值算法（一）
双圆弧插值算法(一) Biarc Interpolation 在游戏开发中经常出现两点间的插值问题.大多数情况下,只需要一个简单的线性插值.线性插值很好,因为不会真的弄错.只有一条可能的线连接这些点. ...
HAL库版STM32双轮自平衡车(三) ———代码精讲
系列文章目录编码电机测速 HAL库OLED的使用 HAL库版STM32双轮自平衡车(一) ---代码思路和PID基础精讲 HAL库版STM32双轮自平衡车(二) --- CubeMX的配置.原理图接 ...
图像插值算法——双立方（三次）卷积插值
双立方(三次)卷积插值是一种数据点插值方法. 在对图像进行缩放,旋转等处理时,有些像素点会因为这些操作变得没有意义,比如二维图像A(2*2)放大为原来的二倍后B(4*4)就会缺失一些像素,如图所示: ...
【图像缩放】双立方（三次）卷积插值
前言图像处理中有三种常用的插值算法: 最邻近插值双线性插值双立方(三次卷积)插值其中效果最好的是双立方(三次卷积)插值,本文介绍它的原理以及使用如果想先看效果和源码,可以拉到最底部本文的契 ...
matlab光顺拐点,基于MATLAB的最大误差双圆弧逼近曲线的算法及实现.pdf
基于MATLAB的最大误差双圆弧逼近曲线的算法及实现.pdf 第31卷第6期基于MⅢB的最大误差双圆弧逼近曲线的算法及实现文章编号:1004-2539120町]06一唧一∞ 基于MAⅡ．AB的最大 ...
python中定义变量有引号和单引号_说说Python 单引号、双引号、三引号的区别？...
公众号新增加了一个栏目,就是每天给大家解答一道Python常见的面试题,反正每天不贪多,一天一题,正好合适,只希望这个面试栏目,给那些正在准备面试的同学,提供一点点帮助! 小猿会从最基础的面试题开始, ...
福利| 一台电脑，双网卡，三个IP地址，如何同时工作
作者丨Caesar 来源丨手机电脑双黑客一台电脑,双网卡,三个IP地址,一个外网有网关,一个专网也有网关.局域网没有网关,我来假设一个. 假设外网和局域网在一个网卡A上面,专网在网卡B上面.如图所示 ...
Python中单引号，双引号，三引号
字符串在任何一种计算机语言中都是非常重要的一种数据类型.Python中表示字符可以有三种写法,分别是单引号,双引号,三引号.它们的区别如下: 单引号 str = 'text' 字符串内容有单引号时需要 ...

双圆弧插值算法（三，代码实现）

双圆弧插值算法（三，代码实现）相关推荐

最新文章

热门文章