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

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

交互式演示             

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

 

 

 代码             

到目前为止，我们只讨论了二维情况。让我们编写一些C++代码来解决三维的情况。它非常相似，除了每个弧可以在不同的平面上对齐。这将在查找旋转方向和平面法线时创建一些调整。经过几次交叉积后，一切都成功了。             

这些代码示例是在以下许可下发布的。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
/******************************************************************************
  Copyright (c) 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:
  
  1. 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.
  
  2. Altered source versions must be plainly marked as such, and must not be
     misrepresented as being the original software.
  
  3. 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.

1
2
3
4
5
6
7
8
//******************************************************************************
//******************************************************************************
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).

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
//******************************************************************************
// Compute the dot product of two vectors.
//******************************************************************************
float Vec_DotProduct(const tVec3 & lhs, const tVec3 & rhs)
{
    return lhs.m_x*rhs.m_x + lhs.m_y*rhs.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_y*rhs.m_z - lhs.m_z*rhs.m_y;
    float y = lhs.m_z*rhs.m_x - lhs.m_x*rhs.m_z;
    float z = lhs.m_x*rhs.m_y - lhs.m_y*rhs.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_x*rhs;
    pResult->m_y = lhs.m_y*rhs;
    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_x*rhsScale;
    pResult->m_y = lhs.m_y + rhs.m_y*rhsScale;
    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.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
//******************************************************************************
// 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.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
//******************************************************************************
// 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.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
//******************************************************************************
// 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 = vDotT*vDotT + denominator*vDotV;
        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∣).

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
//******************************************************************************
// 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, -arcFrac*2.0f + 1.0f);
            }
            else
            {
                // interpolate along the arc
                float angle = arc1.m_angle*arcFrac;
                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, arcFrac*2.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);
            }
        }
    }
}
posted @ 2020-07-14 08:53  吴建明wujianming  阅读(507)  评论(0)    收藏  举报