void btAngularLimit::fit(btScalar& angle) const
{
if (m_halfRange > 0.0f)
{
btScalar relativeAngle = btNormalizeAngle(angle - m_center);
if (!btEqual(relativeAngle, m_halfRange))
{
if (relativeAngle > 0.0f)
{
angle = getHigh();
}
else
{
angle = getLow();
}
}
}
}
int BU_AlgebraicPolynomialSolver::Solve3Cubic(btScalar lead, btScalar a, btScalar b, btScalar c)
{
btScalar p, q, r;
btScalar delta, u, phi;
btScalar dummy;
if (lead != 1.0) {
/* */
/* transform into normal form: x^3 + a x^2 + b x + c = 0 */
/* */
if (btEqual(lead, SIMD_EPSILON)) {
/* */
/* we have a x^2 + b x + c = 0 */
/* */
if (btEqual(a, SIMD_EPSILON)) {
/* */
/* we have b x + c = 0 */
/* */
if (btEqual(b, SIMD_EPSILON)) {
if (btEqual(c, SIMD_EPSILON)) {
return -1;
}
else {
return 0;
}
}
else {
m_roots[0] = -c / b;
return 1;
}
}
else {
p = c / a;
q = b / a;
return Solve2QuadraticFull(a,b,c);
}
}
else {
a = a / lead;
b = b / lead;
c = c / lead;
}
}
/* */
/* we substitute x = y - a / 3 in order to eliminate the quadric term. */
/* we get x^3 + p x + q = 0 */
/* */
a /= 3.0f;
u = a * a;
p = b / 3.0f - u;
q = a * (2.0f * u - b) + c;
/* */
/* now use Cardano's formula */
/* */
if (btEqual(p, SIMD_EPSILON)) {
if (btEqual(q, SIMD_EPSILON)) {
/* */
/* one triple root */
/* */
m_roots[0] = -a;
return 1;
}
else {
/* */
/* one real and two complex roots */
/* */
m_roots[0] = cubic_rt(-q) - a;
return 1;
}
}
q /= 2.0f;
delta = p * p * p + q * q;
if (delta > 0.0f) {
/* */
/* one real and two complex roots. note that v = -p / u. */
/* */
u = -q + btSqrt(delta);
u = cubic_rt(u);
m_roots[0] = u - p / u - a;
return 1;
}
else if (delta < 0.0) {
/* */
/* Casus irreducibilis: we have three real roots */
/* */
r = btSqrt(-p);
p *= -r;
r *= 2.0;
phi = btAcos(-q / p) / 3.0f;
dummy = SIMD_2_PI / 3.0f;
m_roots[0] = r * btCos(phi) - a;
m_roots[1] = r * btCos(phi + dummy) - a;
m_roots[2] = r * btCos(phi - dummy) - a;
return 3;
}
else {
/* */
/* one single and one btScalar root */
/* */
r = cubic_rt(-q);
m_roots[0] = 2.0f * r - a;
m_roots[1] = -r - a;
return 2;
}
}
bool EpaPolyhedron::Create( btPoint3* pInitialPoints,
btPoint3* pSupportPointsOnA, btPoint3* pSupportPointsOnB,
const int nbInitialPoints )
{
#ifndef EPA_POLYHEDRON_USE_PLANES
EPA_DEBUG_ASSERT( ( nbInitialPoints <= 4 ) ,"nbInitialPoints greater than 4!" );
#endif
if ( nbInitialPoints < 4 )
{
// Insufficient nb of points
return false;
}
////////////////////////////////////////////////////////////////////////////////
#ifdef EPA_POLYHEDRON_USE_PLANES
int nbDiffCoords[ 3 ] = { 0, 0, 0 };
bool* pDiffCoords = new bool[ 3 * nbInitialPoints ];
int i;
for (i=0;i<nbInitialPoints*3;i++)
{
pDiffCoords[i] = false;
}
//::memset( pDiffCoords, 0, sizeof( bool ) * 3 * nbInitialPoints );
int axis;
for ( axis = 0; axis < 3; ++axis )
{
for ( int i = 0; i < nbInitialPoints; ++i )
{
bool isDifferent = true;
for ( int j = 0; j < i; ++j )
{
if ( pInitialPoints[ i ][ axis ] == pInitialPoints[ j ][ axis ] )
{
isDifferent = false;
break;
}
}
if ( isDifferent )
{
++nbDiffCoords[ axis ];
pDiffCoords[ axis * nbInitialPoints + i ] = true;
}
}
if ( nbDiffCoords[ axis ] <= 1 )
{
// The input is degenerate
return false;
}
}
int finalPointsIndices[ 4 ] = { -1, -1, -1, -1 };
int axisOrderIndices[ 3 ] = { 0, 1, 2 };
for ( i = 0; i < 2/*round( nbAxis / 2 )*/; ++i )
{
if ( nbDiffCoords[ i ] > nbDiffCoords[ i + 1 ] )
{
int tmp = nbDiffCoords[ i ];
nbDiffCoords[ i ] = nbDiffCoords[ i + 1 ];
nbDiffCoords[ i + 1 ] = tmp;
tmp = axisOrderIndices[ i ];
axisOrderIndices[ i ] = axisOrderIndices[ i + 1 ];
axisOrderIndices[ i + 1 ] = tmp;
}
}
int nbSuccessfullAxis = 0;
// The axes with less different coordinates choose first
int minsIndices[ 3 ] = { -1, -1, -1 };
int maxsIndices[ 3 ] = { -1, -1, -1 };
int finalPointsIndex = 0;
for ( axis = 0; ( axis < 3 ) && ( nbSuccessfullAxis < 2 ); ++axis )
{
int axisIndex = axisOrderIndices[ axis ];
btScalar axisMin = SIMD_INFINITY;
btScalar axisMax = -SIMD_INFINITY;
for ( int i = 0; i < 4; ++i )
{
// Among the diff coords pick the min and max coords
if ( pDiffCoords[ axisIndex * nbInitialPoints + i ] )
{
if ( pInitialPoints[ i ][ axisIndex ] < axisMin )
{
axisMin = pInitialPoints[ i ][ axisIndex ];
minsIndices[ axisIndex ] = i;
}
if ( pInitialPoints[ i ][ axisIndex ] > axisMax )
{
axisMax = pInitialPoints[ i ][ axisIndex ];
maxsIndices[ axisIndex ] = i;
}
}
}
//assert( ( minsIndices[ axisIndex ] != maxsIndices[ axisIndex ] ) &&
// "min and max have the same index!" );
if ( ( minsIndices[ axisIndex ] != -1 ) && ( maxsIndices[ axisIndex ] != -1 ) &&
( minsIndices[ axisIndex ] != maxsIndices[ axisIndex ] ) )
{
++nbSuccessfullAxis;
finalPointsIndices[ finalPointsIndex++ ] = minsIndices[ axisIndex ];
finalPointsIndices[ finalPointsIndex++ ] = maxsIndices[ axisIndex ];
// Make the choosen points to be impossible for other axes to choose
//assert( ( minsIndices[ axisIndex ] != -1 ) && "Invalid index!" );
//assert( ( maxsIndices[ axisIndex ] != -1 ) && "Invalid index!" );
for ( int i = 0; i < 3; ++i )
{
pDiffCoords[ i * nbInitialPoints + minsIndices[ axisIndex ] ] = false;
pDiffCoords[ i * nbInitialPoints + maxsIndices[ axisIndex ] ] = false;
}
}
}
if ( nbSuccessfullAxis <= 1 )
{
// Degenerate input ?
EPA_DEBUG_ASSERT( false ,"nbSuccessfullAxis must be greater than 1!" );
return false;
}
delete[] pDiffCoords;
#endif
//////////////////////////////////////////////////////////////////////////
#ifdef EPA_POLYHEDRON_USE_PLANES
btVector3 v0 = pInitialPoints[ finalPointsIndices[ 1 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
btVector3 v1 = pInitialPoints[ finalPointsIndices[ 2 ] ] - pInitialPoints[ finalPointsIndices[ 0 ] ];
#else
btVector3 v0 = pInitialPoints[ 1 ] - pInitialPoints[ 0 ];
btVector3 v1 = pInitialPoints[ 2 ] - pInitialPoints[ 0 ];
#endif
btVector3 planeNormal = v1.cross( v0 );
planeNormal.normalize();
#ifdef EPA_POLYHEDRON_USE_PLANES
btScalar planeDistance = pInitialPoints[ finalPointsIndices[ 0 ] ].dot( -planeNormal );
#else
btScalar planeDistance = pInitialPoints[ 0 ].dot( -planeNormal );
#endif
#ifdef EPA_POLYHEDRON_USE_PLANES
bool pointOnPlane0 = btEqual( pInitialPoints[ finalPointsIndices[ 0 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane0)
{
EPA_DEBUG_ASSERT(0,"Point0 should be on plane!");
return false;
}
bool pointOnPlane1 = btEqual( pInitialPoints[ finalPointsIndices[ 1 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane1)
{
EPA_DEBUG_ASSERT(0,"Point1 should be on plane!");
return false;
}
bool pointOnPlane2 = btEqual( pInitialPoints[ finalPointsIndices[ 2 ] ].dot( planeNormal ) + planeDistance, PLANE_THICKNESS );
if (!pointOnPlane2)
{
EPA_DEBUG_ASSERT(0,"Point2 should be on plane!");
return false;
}
#endif
#ifndef EPA_POLYHEDRON_USE_PLANES
{
if ( planeDistance > 0 )
{
btVector3 tmp = pInitialPoints[ 1 ];
pInitialPoints[ 1 ] = pInitialPoints[ 2 ];
pInitialPoints[ 2 ] = tmp;
tmp = pSupportPointsOnA[ 1 ];
pSupportPointsOnA[ 1 ] = pSupportPointsOnA[ 2 ];
pSupportPointsOnA[ 2 ] = tmp;
tmp = pSupportPointsOnB[ 1 ];
pSupportPointsOnB[ 1 ] = pSupportPointsOnB[ 2 ];
pSupportPointsOnB[ 2 ] = tmp;
}
}
EpaVertex* pVertexA = CreateVertex( pInitialPoints[ 0 ], pSupportPointsOnA[ 0 ], pSupportPointsOnB[ 0 ] );
EpaVertex* pVertexB = CreateVertex( pInitialPoints[ 1 ], pSupportPointsOnA[ 1 ], pSupportPointsOnB[ 1 ] );
EpaVertex* pVertexC = CreateVertex( pInitialPoints[ 2 ], pSupportPointsOnA[ 2 ], pSupportPointsOnB[ 2 ] );
EpaVertex* pVertexD = CreateVertex( pInitialPoints[ 3 ], pSupportPointsOnA[ 3 ], pSupportPointsOnB[ 3 ] );
#else
finalPointsIndices[ 3 ] = -1;
btScalar absMaxDist = -SIMD_INFINITY;
btScalar maxDist;
for ( int pointIndex = 0; pointIndex < nbInitialPoints; ++pointIndex )
{
btScalar dist = planeNormal.dot( pInitialPoints[ pointIndex ] ) + planeDistance;
btScalar absDist = abs( dist );
if ( ( absDist > absMaxDist ) &&
!btEqual( dist, PLANE_THICKNESS ) )
{
absMaxDist = absDist;
maxDist = dist;
finalPointsIndices[ 3 ] = pointIndex;
}
}
if ( finalPointsIndices[ 3 ] == -1 )
{
Destroy();
return false;
}
if ( maxDist > PLANE_THICKNESS )
{
// Can swap indices only
btPoint3 tmp = pInitialPoints[ finalPointsIndices[ 1 ] ];
pInitialPoints[ finalPointsIndices[ 1 ] ] = pInitialPoints[ finalPointsIndices[ 2 ] ];
pInitialPoints[ finalPointsIndices[ 2 ] ] = tmp;
tmp = pSupportPointsOnA[ finalPointsIndices[ 1 ] ];
pSupportPointsOnA[ finalPointsIndices[ 1 ] ] = pSupportPointsOnA[ finalPointsIndices[ 2 ] ];
pSupportPointsOnA[ finalPointsIndices[ 2 ] ] = tmp;
tmp = pSupportPointsOnB[ finalPointsIndices[ 1 ] ];
pSupportPointsOnB[ finalPointsIndices[ 1 ] ] = pSupportPointsOnB[ finalPointsIndices[ 2 ] ];
pSupportPointsOnB[ finalPointsIndices[ 2 ] ] = tmp;
}
EpaVertex* pVertexA = CreateVertex( pInitialPoints[ finalPointsIndices[ 0 ] ], pSupportPointsOnA[ finalPointsIndices[ 0 ] ], pSupportPointsOnB[ finalPointsIndices[ 0 ] ] );
EpaVertex* pVertexB = CreateVertex( pInitialPoints[ finalPointsIndices[ 1 ] ], pSupportPointsOnA[ finalPointsIndices[ 1 ] ], pSupportPointsOnB[ finalPointsIndices[ 1 ] ] );
EpaVertex* pVertexC = CreateVertex( pInitialPoints[ finalPointsIndices[ 2 ] ], pSupportPointsOnA[ finalPointsIndices[ 2 ] ], pSupportPointsOnB[ finalPointsIndices[ 2 ] ] );
EpaVertex* pVertexD = CreateVertex( pInitialPoints[ finalPointsIndices[ 3 ] ], pSupportPointsOnA[ finalPointsIndices[ 3 ] ], pSupportPointsOnB[ finalPointsIndices[ 3 ] ] );
#endif
EpaFace* pFaceA = CreateFace();
EpaFace* pFaceB = CreateFace();
EpaFace* pFaceC = CreateFace();
EpaFace* pFaceD = CreateFace();
EpaHalfEdge* pFaceAHalfEdges[ 3 ];
EpaHalfEdge* pFaceCHalfEdges[ 3 ];
EpaHalfEdge* pFaceBHalfEdges[ 3 ];
EpaHalfEdge* pFaceDHalfEdges[ 3 ];
pFaceAHalfEdges[ 0 ] = CreateHalfEdge();
pFaceAHalfEdges[ 1 ] = CreateHalfEdge();
pFaceAHalfEdges[ 2 ] = CreateHalfEdge();
pFaceBHalfEdges[ 0 ] = CreateHalfEdge();
pFaceBHalfEdges[ 1 ] = CreateHalfEdge();
pFaceBHalfEdges[ 2 ] = CreateHalfEdge();
pFaceCHalfEdges[ 0 ] = CreateHalfEdge();
pFaceCHalfEdges[ 1 ] = CreateHalfEdge();
pFaceCHalfEdges[ 2 ] = CreateHalfEdge();
pFaceDHalfEdges[ 0 ] = CreateHalfEdge();
pFaceDHalfEdges[ 1 ] = CreateHalfEdge();
pFaceDHalfEdges[ 2 ] = CreateHalfEdge();
pFaceA->m_pHalfEdge = pFaceAHalfEdges[ 0 ];
pFaceB->m_pHalfEdge = pFaceBHalfEdges[ 0 ];
pFaceC->m_pHalfEdge = pFaceCHalfEdges[ 0 ];
pFaceD->m_pHalfEdge = pFaceDHalfEdges[ 0 ];
pFaceAHalfEdges[ 0 ]->m_pNextCCW = pFaceAHalfEdges[ 1 ];
pFaceAHalfEdges[ 1 ]->m_pNextCCW = pFaceAHalfEdges[ 2 ];
pFaceAHalfEdges[ 2 ]->m_pNextCCW = pFaceAHalfEdges[ 0 ];
pFaceBHalfEdges[ 0 ]->m_pNextCCW = pFaceBHalfEdges[ 1 ];
pFaceBHalfEdges[ 1 ]->m_pNextCCW = pFaceBHalfEdges[ 2 ];
pFaceBHalfEdges[ 2 ]->m_pNextCCW = pFaceBHalfEdges[ 0 ];
pFaceCHalfEdges[ 0 ]->m_pNextCCW = pFaceCHalfEdges[ 1 ];
pFaceCHalfEdges[ 1 ]->m_pNextCCW = pFaceCHalfEdges[ 2 ];
pFaceCHalfEdges[ 2 ]->m_pNextCCW = pFaceCHalfEdges[ 0 ];
pFaceDHalfEdges[ 0 ]->m_pNextCCW = pFaceDHalfEdges[ 1 ];
pFaceDHalfEdges[ 1 ]->m_pNextCCW = pFaceDHalfEdges[ 2 ];
pFaceDHalfEdges[ 2 ]->m_pNextCCW = pFaceDHalfEdges[ 0 ];
pFaceAHalfEdges[ 0 ]->m_pFace = pFaceA;
pFaceAHalfEdges[ 1 ]->m_pFace = pFaceA;
pFaceAHalfEdges[ 2 ]->m_pFace = pFaceA;
pFaceBHalfEdges[ 0 ]->m_pFace = pFaceB;
pFaceBHalfEdges[ 1 ]->m_pFace = pFaceB;
pFaceBHalfEdges[ 2 ]->m_pFace = pFaceB;
pFaceCHalfEdges[ 0 ]->m_pFace = pFaceC;
pFaceCHalfEdges[ 1 ]->m_pFace = pFaceC;
pFaceCHalfEdges[ 2 ]->m_pFace = pFaceC;
pFaceDHalfEdges[ 0 ]->m_pFace = pFaceD;
pFaceDHalfEdges[ 1 ]->m_pFace = pFaceD;
pFaceDHalfEdges[ 2 ]->m_pFace = pFaceD;
pFaceAHalfEdges[ 0 ]->m_pVertex = pVertexA;
pFaceAHalfEdges[ 1 ]->m_pVertex = pVertexB;
pFaceAHalfEdges[ 2 ]->m_pVertex = pVertexC;
pFaceBHalfEdges[ 0 ]->m_pVertex = pVertexB;
pFaceBHalfEdges[ 1 ]->m_pVertex = pVertexD;
pFaceBHalfEdges[ 2 ]->m_pVertex = pVertexC;
pFaceCHalfEdges[ 0 ]->m_pVertex = pVertexD;
pFaceCHalfEdges[ 1 ]->m_pVertex = pVertexA;
pFaceCHalfEdges[ 2 ]->m_pVertex = pVertexC;
pFaceDHalfEdges[ 0 ]->m_pVertex = pVertexB;
pFaceDHalfEdges[ 1 ]->m_pVertex = pVertexA;
pFaceDHalfEdges[ 2 ]->m_pVertex = pVertexD;
//pVertexA->m_pHalfEdge = pFaceAHalfEdges[ 0 ];
//pVertexB->m_pHalfEdge = pFaceAHalfEdges[ 1 ];
//pVertexC->m_pHalfEdge = pFaceAHalfEdges[ 2 ];
//pVertexD->m_pHalfEdge = pFaceBHalfEdges[ 1 ];
pFaceAHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 0 ];
pFaceAHalfEdges[ 1 ]->m_pTwin = pFaceBHalfEdges[ 2 ];
pFaceAHalfEdges[ 2 ]->m_pTwin = pFaceCHalfEdges[ 1 ];
pFaceBHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 2 ];
pFaceBHalfEdges[ 1 ]->m_pTwin = pFaceCHalfEdges[ 2 ];
pFaceBHalfEdges[ 2 ]->m_pTwin = pFaceAHalfEdges[ 1 ];
pFaceCHalfEdges[ 0 ]->m_pTwin = pFaceDHalfEdges[ 1 ];
pFaceCHalfEdges[ 1 ]->m_pTwin = pFaceAHalfEdges[ 2 ];
pFaceCHalfEdges[ 2 ]->m_pTwin = pFaceBHalfEdges[ 1 ];
pFaceDHalfEdges[ 0 ]->m_pTwin = pFaceAHalfEdges[ 0 ];
pFaceDHalfEdges[ 1 ]->m_pTwin = pFaceCHalfEdges[ 0 ];
pFaceDHalfEdges[ 2 ]->m_pTwin = pFaceBHalfEdges[ 0 ];
if ( !pFaceA->Initialize() || !pFaceB->Initialize() ||
!pFaceC->Initialize() || !pFaceD->Initialize() )
{
EPA_DEBUG_ASSERT( false, "One initial face failed to initialize!" );
return false;
}
#ifdef EPA_POLYHEDRON_USE_PLANES
if ( nbInitialPoints > 4 )
{
for ( int i = 0; i < nbInitialPoints; ++i )
{
if ( ( i != finalPointsIndices[ 0 ] ) && ( i != finalPointsIndices[ 1 ] ) &&
( i != finalPointsIndices[ 2 ] ) && ( i != finalPointsIndices[ 3 ] ) )
{
std::list< EpaFace* >::iterator facesItr( m_faces.begin() );
while ( facesItr != m_faces.end() )
{
EpaFace* pFace = *facesItr;
btScalar dist = pFace->m_planeNormal.dot( pInitialPoints[ i ] ) + pFace->m_planeDistance;
if ( dist > PLANE_THICKNESS )
{
std::list< EpaFace* > newFaces;
bool expandOk = Expand( pInitialPoints[ i ], pSupportPointsOnA[ i ], pSupportPointsOnB[ i ],
pFace, newFaces );
if ( !expandOk )
{
// One or more new faces are affinely dependent
return false;
}
EPA_DEBUG_ASSERT( !newFaces.empty() ,"Polyhedron should have expanded!" );
break;
}
++facesItr;
}
}
}
}
#endif
return true;
}
int BU_AlgebraicPolynomialSolver::Solve4Quartic(btScalar lead, btScalar a, btScalar b, btScalar c, btScalar d)
{
btScalar p, q ,r;
btScalar u, v, w;
int i, num_roots, num_tmp;
//btScalar tmp[2];
if (lead != 1.0) {
/* */
/* transform into normal form: x^4 + a x^3 + b x^2 + c x + d = 0 */
/* */
if (btEqual(lead, SIMD_EPSILON)) {
/* */
/* we have a x^3 + b x^2 + c x + d = 0 */
/* */
if (btEqual(a, SIMD_EPSILON)) {
/* */
/* we have b x^2 + c x + d = 0 */
/* */
if (btEqual(b, SIMD_EPSILON)) {
/* */
/* we have c x + d = 0 */
/* */
if (btEqual(c, SIMD_EPSILON)) {
if (btEqual(d, SIMD_EPSILON)) {
return -1;
}
else {
return 0;
}
}
else {
m_roots[0] = -d / c;
return 1;
}
}
else {
p = c / b;
q = d / b;
return Solve2QuadraticFull(b,c,d);
}
}
else {
return Solve3Cubic(1.0, b / a, c / a, d / a);
}
}
else {
a = a / lead;
b = b / lead;
c = c / lead;
d = d / lead;
}
}
/* */
/* we substitute x = y - a / 4 in order to eliminate the cubic term. */
/* we get: y^4 + p y^2 + q y + r = 0. */
/* */
a /= 4.0f;
p = b - 6.0f * a * a;
q = a * (8.0f * a * a - 2.0f * b) + c;
r = a * (a * (b - 3.f * a * a) - c) + d;
if (btEqual(q, SIMD_EPSILON)) {
/* */
/* biquadratic equation: y^4 + p y^2 + r = 0. */
/* */
num_roots = Solve2Quadratic(p, r);
if (num_roots > 0) {
if (m_roots[0] > 0.0f) {
if (num_roots > 1) {
if ((m_roots[1] > 0.0f) && (m_roots[1] != m_roots[0])) {
u = btSqrt(m_roots[1]);
m_roots[2] = u - a;
m_roots[3] = -u - a;
u = btSqrt(m_roots[0]);
m_roots[0] = u - a;
m_roots[1] = -u - a;
return 4;
}
else {
u = btSqrt(m_roots[0]);
m_roots[0] = u - a;
m_roots[1] = -u - a;
return 2;
}
}
else {
u = btSqrt(m_roots[0]);
m_roots[0] = u - a;
m_roots[1] = -u - a;
return 2;
}
}
}
return 0;
}
else if (btEqual(r, SIMD_EPSILON)) {
/* */
/* no absolute term: y (y^3 + p y + q) = 0. */
/* */
num_roots = Solve3Cubic(1.0, 0.0, p, q);
for (i = 0; i < num_roots; ++i) m_roots[i] -= a;
if (num_roots != -1) {
m_roots[num_roots] = -a;
++num_roots;
}
else {
m_roots[0] = -a;
num_roots = 1;;
}
return num_roots;
}
else {
/* */
/* we solve the resolvent cubic equation */
/* */
num_roots = Solve3Cubic(1.0f, -0.5f * p, -r, 0.5f * r * p - 0.125f * q * q);
if (num_roots == -1) {
num_roots = 1;
m_roots[0] = 0.0f;
}
/* */
/* build two quadric equations */
/* */
w = m_roots[0];
u = w * w - r;
v = 2.0f * w - p;
if (btEqual(u, SIMD_EPSILON))
u = 0.0;
else if (u > 0.0f)
u = btSqrt(u);
else
return 0;
if (btEqual(v, SIMD_EPSILON))
v = 0.0;
else if (v > 0.0f)
v = btSqrt(v);
else
return 0;
if (q < 0.0f) v = -v;
w -= u;
num_roots=Solve2Quadratic(v, w);
for (i = 0; i < num_roots; ++i)
{
m_roots[i] -= a;
}
w += 2.0f *u;
btScalar tmp[2];
tmp[0] = m_roots[0];
tmp[1] = m_roots[1];
num_tmp = Solve2Quadratic(-v, w);
for (i = 0; i < num_tmp; ++i)
{
m_roots[i + num_roots] = tmp[i] - a;
m_roots[i]=tmp[i];
}
return (num_tmp + num_roots);
}
}
