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); } }