spieleprogrammierer.de - Forum und Wiki zur Spieleprogrammierung und Spieleentwicklung

C-/C++-Quelltext

//Gibt an ob ein linksorientierter (D3D) Vektorraum oder ein rechtsorientierter (OGL) verwendet wird

const bool              c_bLeftOrientedCoordSystem              = true;
//Soll zur Kollisionserkennung ein Kollisionsvorhersagesystem verwendet werden?

const bool              c_bUseCollisionPrediction               = true;
//Gibt die maximale Anzahl der globalen Kräfte pro Frame an

const unsigned int      c_iMaxGlobalForces                      = 100;
//Entscheidet, ab welcher Anzahl von Vertizes der HillClimbing-Query

//dem linearen BruteForce vorgezogen werden soll

//Varname steht für BreakEvenPointForExtremeVertexQuery

const unsigned int      c_iBEPForExtremeVertexQuery             = 40;
//Gibt die allg. zu verwendende FloatingPoint Toleranz an

const float             c_fFloatingPointTolerance               = 0.0001f;
//Wenn die relative Geschwindigkeit zweier Kontaktpunkte in Normalrichtung betragsmäßig kleiner ist

//als dieser Wert, handelt es sich um einen Resting-Kontakt!

const float             c_fRestingContactTolerance              = sqrtf(19.62f * c_fFloatingPointTolerance);
//Gibt den Standard-Elastizitätsfaktor an

const float             c_fStdElasticyCoeff                     = 0.8f;
//Toleranzwert für welchen eine Rückstoßgeschwindigkeit als erreicht betrachtet wird

const float             c_fReimpulsiveVelocityTolerance         = 0.0001f;
//Maximale Anzahl der Iterationen die verwendet werden dürfen um alle Kollisions aufzulösen! 

const unsigned int      c_iMaxCollisionImpulsIterations         = 5000;
//Maximale Anzahl der Iterationen die verwendet werden dürfen um alle RestingContacts aufzulösen! 

const unsigned int      c_iMaxRestingContactIterations          = 5000;
//Toleranzwert, ab welchem eine Gelenksbedingung als aufgelöst gilt

const float             c_fJointTolerance                       = 0.0001f;
//Standardwert für Gleitreibungskoeffizient

const float             c_fStdDynamicFrictionCoeff              = 0.01f;
//Standardwert für Haftreibungskoeffizient

const float             c_fStdStaticFrictionCoeff               = 0.02f;

C-/C++-Quelltext

const bool ogpCollisionResponse::DoCollisionResponse(ogpContactList* pFirstContactPoint, const UINT iCPCount, 
                                                     const float fStepSize)
{
    if(iCPCount <= 0) return true;
    //Falls nötig Arrays vergrößern!

    if(iCPCount > m_iTempInfoSize)
    {
        m_iTempInfoSize = iCPCount;
        if(m_pTempInfo) OG_SAFE_DELETE_ARRAY(m_pTempInfo);
        m_pTempInfo = new STempInfos[m_iTempInfoSize];
    }
    if(iCPCount > m_iContactArraySize)
    {
        m_iContactArraySize = iCPCount;
        if(m_pContactArray) OG_SAFE_DELETE_ARRAY(m_pContactArray);
        m_pContactArray = new ogpContact[m_iContactArraySize];
    }

    //Liste der Kontakte in Array kopieren

    ogpContactList* pContact = pFirstContactPoint;
    for(UINT i = 0; i < iCPCount; ++i) {m_pContactArray[i] = pContact->Contact; pContact = pContact->pNext;}

    //Bleibende und kollidierende Kontakte trennen. Kollidierende am Anfang und bleibende am Ende des Arrays

    UINT iCCCount = iCPCount;
    for(int i = 0; i < static_cast<int>(iCCCount); ++i)
    {
        const og3DVector urel   = m_pContactArray[i].pA->CalcPointVelocity(m_pContactArray[i].vPoint) - 
                                  m_pContactArray[i].pB->CalcPointVelocity(m_pContactArray[i].vPoint);
        const float fUrelDotN   = og3DVectorDot(urel, m_pContactArray[i].vNormal);
        if(fabsf(fUrelDotN) < c_fRestingContactTolerance)
        {
            //RestingContact!

            const ogpContact Temp       = m_pContactArray[i];
            m_pContactArray[i--]        = m_pContactArray[--iCCCount];
            m_pContactArray[iCCCount]   = Temp;
        }
    }
    const UINT iRCCount = iCPCount - iCCCount;

    //Kollisionsauflösung für kollidierende Kontakte

    if(iCCCount > 0)
    {
        if(!DoCRForCollidingContact(m_pContactArray, iCCCount)) 
            OG_WARNING(L"CollidingContact-Auflösung wurde nicht ordnungsgemäß beendet!");
    }

    //Kollisionsauflösung für bleibende Kontakte

    if(iRCCount > 0)
    {
        if(!DoCRForRestingContact(m_pContactArray + iCCCount, iRCCount, fStepSize)) 
            OG_WARNING(L"RestingContact-Auflösung wurde nicht ordnungsgemäß beendet!");
    }

    return true;
}

//-----------------------------------------------------------------//


const bool ogpCollisionResponse::DoCRForCollidingContact(ogpContact* pContacts, const UINT iCPCount)
{
    //Folgende Daten müssen nur einmal berechnet werden!

    for(UINT i = 0; i < iCPCount; ++i)
    {
        //Relative Geschwindkeit berechnen

        const og3DVector urel   = pContacts[i].pA->CalcPointVelocity(pContacts[i].vPoint) -
                                  pContacts[i].pB->CalcPointVelocity(pContacts[i].vPoint);
        const og3DVector vUrel  = og3DVectorDot(urel, pContacts[i].vNormal) * pContacts[i].vNormal;

        m_pTempInfo[i].fElasticity = pContacts[i].pA->GetElasticyCoeff() * pContacts[i].pB->GetElasticyCoeff();
        m_pTempInfo[i].vTargetUrel = -m_pTempInfo[i].fElasticity * vUrel;
        m_pTempInfo[i].K = pContacts[i].pA->CalcKMatrix(pContacts[i].vPoint, pContacts[i].vPoint) +
                           pContacts[i].pB->CalcKMatrix(pContacts[i].vPoint, pContacts[i].vPoint);

        //Achse in Normalrichtung schauen lassen

        if(og3DVectorDot(pContacts[i].vNormal, pContacts[i].vAxis) < 0.f) pContacts[i].vAxis = -pContacts[i].vAxis;
    }

    //iOk speichert die Anzahl der Kontaktpunkte, welche erfolgreich aufgelöst wurden!

    //Bei einer Impulsanwendung wird iOk auf 1 gesetzt!

    UINT            iOk                 = 0;
    UINT            iIndex              = 0;
    UINT            iIterations         = 0;
    og3DVector      vImpulsSum          = 0.f;
    while(iOk++ < iCPCount)
    {
        //Relative Geschwindigkeit der beiden Kontaktpunkte berechnen!

        const og3DVector urel   = pContacts[iIndex].pA->CalcPointVelocity(pContacts[iIndex].vPoint) - 
                                  pContacts[iIndex].pB->CalcPointVelocity(pContacts[iIndex].vPoint);
        const float fUrelDotN   = og3DVectorDot(urel, pContacts[iIndex].vNormal);
        const og3DVector vUrel  = fUrelDotN * pContacts[iIndex].vNormal;

        //1. Bedingung: Gesamtimpuls gleich Null, und Zielgeschwindigkeit überschritten --> Erfolgreiche Kollisionsauflösung

        if(vImpulsSum != og3DVector(0.f) || fUrelDotN <= og3DVectorDot(m_pTempInfo[iIndex].vTargetUrel, pContacts[iIndex].vNormal))
        {
            const og3DVector vDeltaUrel = m_pTempInfo[iIndex].vTargetUrel - vUrel;
            const float fDotDeltaUrel   = og3DVectorDot(vDeltaUrel, pContacts[iIndex].vNormal);
            //2. Bedingung: Zielgeschwindigkeit bereits erreicht!

            if(fDotDeltaUrel > c_fReimpulsiveVelocityTolerance)
            {
                //Abstoßenden Impuls berechnen und anwenden

                const float fVectorMatrixProduct = ogUtilsMatrixVectorProduct(pContacts[iIndex].vNormal, m_pTempInfo[iIndex].K);
                og3DVector p = vDeltaUrel;
                if(fabsf(fVectorMatrixProduct) > c_fFloatingPointTolerance) p /= fVectorMatrixProduct;
                pContacts[iIndex].pA->ApplyImpuls(p, pContacts[iIndex].vPoint);
                pContacts[iIndex].pB->ApplyImpuls(-p, pContacts[iIndex].vPoint);
                vImpulsSum += p;
                iOk = 1;
            }
            else if(fDotDeltaUrel < -c_fReimpulsiveVelocityTolerance && 
                    og3DVectorDot(vImpulsSum, pContacts[iIndex].vNormal) > -c_fFloatingPointTolerance)
            {
                //Anziehenden KorrekturImpuls berechnen...

                const float fVectorMatrixProduct = ogUtilsMatrixVectorProduct(pContacts[iIndex].vNormal, m_pTempInfo[iIndex].K);
                og3DVector p = vDeltaUrel;
                if(fabsf(fVectorMatrixProduct) > c_fFloatingPointTolerance) p /= fVectorMatrixProduct;
                if(og3DVectorDot(vImpulsSum + p, pContacts[iIndex].vNormal) < -c_fFloatingPointTolerance)
                {
                    //Impuls zu stark --> maximal gültigen anwenden!

                    p = -vImpulsSum;
                }
                //... und anwenden

                pContacts[iIndex].pA->ApplyImpuls(p, pContacts[iIndex].vPoint);
                pContacts[iIndex].pB->ApplyImpuls(-p, pContacts[iIndex].vPoint);
                vImpulsSum += p;
                iOk = 1;
            }
        }

        //Maximale Anzahl an Iterationen erreicht? --> Abbruch!

        if(++iIterations > c_iMaxCollisionImpulsIterations) return false;
        //Kontaktpunkte werden wie ein Ringbuffer immer wieder durchlaufen!

        if(++iIndex >= iCPCount) iIndex = 0;
    }

    return true;
}

//-----------------------------------------------------------------//


//ANMERKUNG: fStepSize soll den künftigen Zeitschritt approximieren. Es

//ist der aktuelle Zeitschritt und es wird davon ausgegangen, dass sich

//die Schrittweite nicht groß ändert! (optimale Lösung --> FrameBasedRendering)

const bool ogpCollisionResponse::DoCRForRestingContact(ogpContact* pContacts, const UINT iCPCount, 
                                                       const float fStepSize)
{
    //Folgende Daten müssen nur einmal berechnet werden!

    for(UINT i = 0; i < iCPCount; ++i)
    {
        m_pTempInfo[i].K = pContacts[i].pA->CalcKMatrix(pContacts[i].vPoint, pContacts[i].vPoint) +
                           pContacts[i].pB->CalcKMatrix(pContacts[i].vPoint, pContacts[i].vPoint);
        //Achse in Normalrichtung schauen lassen

        if(og3DVectorDot(pContacts[i].vNormal, pContacts[i].vAxis) < 0.f) pContacts[i].vAxis = -pContacts[i].vAxis;
    }

    //iOk speichert die Anzahl der Kontaktpunkte, welche erfolgreich aufgelöst wurden!

    UINT            iOk                 = 0;
    UINT            iIndex              = 0;
    UINT            iIterations         = 0;
    og3DVector      vImpulsSum          = 0.f;
    while(iOk++ < iCPCount)
    {
        //Wenn Körper ineinander driften, dann Zeit Geschwindigkeit in Normalrichtung

        const og3DVector vrel   = pContacts[iIndex].pB->GetVelocity() - pContacts[iIndex].pA->GetVelocity();
        //Positiv, falls Körper ineinander driften

        const float fVn         = og3DVectorDot(vrel, pContacts[iIndex].vAxis);
        //N * d < 0, falls sich Körper schneiden!

        const og3DVector d      = (pContacts[iIndex].fDistance - fVn * fStepSize) * pContacts[iIndex].vAxis;
        //Positiv, wenn Körper sich überlappen

        const float fdDotN      = og3DVectorDot(-d, pContacts[iIndex].vNormal);

        //1. Bedingung: GesamtImpuls gleich Null, und Abstand in Normalenrichtung positiv!

        if(vImpulsSum != og3DVector(0.f) || fdDotN > -c_fJointTolerance)
        {
            //2. Bedingung: Abgstand klein genug, dass als erfüllt angesehen werden kann

            if(fabsf(fdDotN) > c_fJointTolerance)
            {
                //Positions-Korrektur-Impuls berechnen

                const float fVectorMatrixProduct = ogUtilsMatrixVectorProduct(pContacts[iIndex].vNormal, m_pTempInfo[iIndex].K);
                og3DVector p = (fdDotN * pContacts[iIndex].vNormal) / (fVectorMatrixProduct * fStepSize);
                if(fdDotN < 0.f && og3DVectorDot(vImpulsSum + p, pContacts[iIndex].vNormal) < -c_fFloatingPointTolerance)
                {
                    //Impuls zu stark --> maximal gültigen anwenden!

                    p = -vImpulsSum;
                }
                //und anweden!

                pContacts[iIndex].pA->ApplyImpuls(p, pContacts[iIndex].vPoint);
                pContacts[iIndex].pB->ApplyImpuls(-p, pContacts[iIndex].vPoint);

                vImpulsSum += p;
                iOk = 1;
            }
        }

        if(++iIterations > c_iMaxRestingContactIterations) return false;
        if(++iIndex >= iCPCount) iIndex = 0;
    }

    return true;
}