Biarc Interpolation

When the quadratic formula is computable, one of the solutions is positive and the other is negative. We always want the positive solution because that represents the shorter arc. The denominator is always positive and the discriminant is going to be greater than \((v \cdot t)^2\) so choosing the plus version will give a positive result.

\(d_2 = {\Large \frac{ -(v \cdot t) + \sqrt { (v \cdot t)^2 + 2 (1 - t_1 \cdot t_2)(v \cdot v) } }{ 2 (1 - t_1 \cdot t_2) } }\)

Because the tangents are normalized, all inputs create a discriminant greater than or equal to zero. Thus we don't need to worry about the square root. However, the denominator can evaluate to zero when the tangents are equal. To support this case we can solve for \(d_2\) again by letting \(t_1 = t_2\).

\( d_2 = \frac { \frac {1}{2} v \cdot v - d_2 v \cdot t_2} { v \cdot t_2 - d_2 (t_2 \cdot t_2 - 1) }\)

\( d_2 = \frac { \frac {1}{2} v \cdot v - d_2 v \cdot t_2} { v \cdot t_2 }\)

\( d_2 (v \cdot t_2) = \frac {1}{2} v \cdot v - d_2 v \cdot t_1\)

\( d_2 (v \cdot t_2) + d_2 v \cdot t_1 = \frac {1}{2} v \cdot v \)

\( 2 d_2 v \cdot t_2 = \frac {v \cdot v}{2} \)

\( d_2 = \frac {v \cdot v}{4 v \cdot t_2 } \)

The final edge case we need to handle occurs when the tangents are equal and \(v\) is perpendicular to them. This will create a zero denominator in the solution for case 2 and implies that \(d_1\) and \(d_2\) are at an infinite distance. This can be handled with two semicircles as seen here:

\(p_m = p_1 + \frac{v}{2}\) \(c_1 = p_1 + \frac{v}{4}\) \(c_2 = p_1 + \frac{3v}{4}\) \(r_1 = \frac{\lvert v \rvert}{4}\) \(r_2 = r_1\) \(\theta_1 = \begin{cases} \pi & v \times t_2 \lt 0 \\ -\pi & \text{otherwise} \end{cases}\) \(\theta_2 = \begin{cases} \pi & v \times t_2 \gt 0 \\ -\pi & \text{otherwise} \end{cases} \)

Code

So far, we've just discussed the two dimensional case. Let's write some C++ code to solve the three dimensional case. It's pretty similar except that each of the arcs can be aligned on a different plane. This creates some adjustments in finding the rotation direction and the plane normal. After a few cross products, it all works out.

These code samples are released under the following license.

/******************************************************************************
  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.

//******************************************************************************
//******************************************************************************
struct tVec3
{
    float x;
    float y;
    float 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.x*rhs.x + lhs.y*rhs.y + lhs.z*rhs.z;
}

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

    pResult->x = x;
    pResult->y = y;
    pResult->z = z;
}

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

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

//******************************************************************************
// Compute a scaled vector.
//******************************************************************************
void Vec_Scale(tVec3* pResult, const tVec3& lhs, float rhs)
{
    pResult->x = lhs.x*rhs;
    pResult->y = lhs.y*rhs;
    pResult->z = lhs.z*rhs;
}

//******************************************************************************
// Add a vector to a scaled vector.
//******************************************************************************
void Vec_AddScaled(tVec3* pResult, const tVec3& lhs, const tVec3& rhs, float rhsScale)
{
    pResult->x = lhs.x + rhs.x*rhsScale;
    pResult->y = lhs.y + rhs.y*rhsScale;
    pResult->z = lhs.z + rhs.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
        // 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 \(d_1\) and \(d_2\) 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 = 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 \gt \frac{\pi}{2} \lvert p_2 - p_1 \rvert \)).

//******************************************************************************
// 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 from c+axis1 to c-axis1
                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, arc2.m_center, arc2.m_axis1);
        }
        else
        {
            const float arcFrac = (fracDist-arc1.m_arcLen) / arc2.m_arcLen;
            if (arc2.m_radius == 0.0f)
            {
                // interpolate along the line from c-axis1 to c+axis1
                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);
            }
        }
    }
}