50 lines
1.1 KiB
C++
50 lines
1.1 KiB
C++
// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
|
|
// SPDX-FileCopyrightText: 2021 Jorrit Rouwe
|
|
// SPDX-License-Identifier: MIT
|
|
|
|
#pragma once
|
|
|
|
JPH_NAMESPACE_BEGIN
|
|
|
|
/// Find the roots of \f$inA \: x^2 + inB \: x + inC = 0\f$.
|
|
/// @return The number of roots, actual roots in outX1 and outX2.
|
|
/// If number of roots returned is 1 then outX1 == outX2.
|
|
template <typename T>
|
|
inline int FindRoot(const T inA, const T inB, const T inC, T &outX1, T &outX2)
|
|
{
|
|
// Check if this is a linear equation
|
|
if (inA == T(0))
|
|
{
|
|
// Check if this is a constant equation
|
|
if (inB == T(0))
|
|
{
|
|
if (inC == T(0))
|
|
{
|
|
outX1 = outX2 = 0.0f;
|
|
return 1; // Actually infinitely many solutions
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
// Linear equation with 1 solution
|
|
outX1 = outX2 = -inC / inB;
|
|
return 1;
|
|
}
|
|
|
|
// See Numerical Recipes in C, Chapter 5.6 Quadratic and Cubic Equations
|
|
T det = DifferenceOfProducts(inB, inB, T(4) * inA, inC);
|
|
if (det < T(0))
|
|
return 0;
|
|
T q = (inB + Sign(inB) * Sqrt(det)) / T(-2);
|
|
outX1 = q / inA;
|
|
if (q == T(0))
|
|
{
|
|
outX2 = outX1;
|
|
return 1;
|
|
}
|
|
outX2 = inC / q;
|
|
return 2;
|
|
}
|
|
|
|
JPH_NAMESPACE_END
|