コード例 #1
0
ファイル: Collision.cpp プロジェクト: PaulC88/GamaGameEngine
	void polygontoPolygon(Manifold* m, RigidBody* a, RigidBody* b) {
		Polygon* A = reinterpret_cast<Polygon *>(a->shape);
		Polygon* B = reinterpret_cast<Polygon *>(b->shape);
		m->contactCount = 0;

		unsigned int faceA;
		float penetrationA = findAxisLeastPenetration(&faceA, A, B);
		if (penetrationA >= 0.0f)
			return;

		unsigned int faceB;
		float penetrationB = findAxisLeastPenetration(&faceB, B, A);
		if (penetrationB >= 0.0f)
			return;

		unsigned int referenceIndex;
		bool flip; 

		Polygon* RefPoly; 
		Polygon* IncPoly; 

		if (BiasGreaterThan(penetrationA, penetrationB)) {
			RefPoly = A;
			IncPoly = B;
			referenceIndex = faceA;
			flip = false;
		} 
		else {
			RefPoly = B;
			IncPoly = A;
			referenceIndex = faceB;
			flip = true;
		}

		vec2 incidentFace[2];
		findIncidentFace(incidentFace, RefPoly, IncPoly, referenceIndex);

		vec2 v1 = RefPoly->m_vertices[referenceIndex];
		referenceIndex = referenceIndex + 1 == RefPoly->m_vertexCount ? 0 : referenceIndex + 1;
		vec2 v2 = RefPoly->m_vertices[referenceIndex];

		v1 = RefPoly->m_orientation * v1 + RefPoly->m_body->getPosition();
		v2 = RefPoly->m_orientation * v2 + RefPoly->m_body->getPosition();

		vec2 sidePlaneNormal = (v2 - v1);
		sidePlaneNormal.normalise();

		vec2 refFaceNormal(sidePlaneNormal.y, -sidePlaneNormal.x);

		float refC = dot(refFaceNormal, v1);
		float negSide = -dot(sidePlaneNormal, v1);
		float posSide = dot(sidePlaneNormal, v2);

		if (clip(-sidePlaneNormal, negSide, incidentFace) < 2)
			return;

		if (clip(sidePlaneNormal, posSide, incidentFace) < 2)
			return;

		m->normal = flip ? -refFaceNormal : refFaceNormal;

		unsigned int cp = 0; 
		float separation = dot(refFaceNormal, incidentFace[0]) - refC;
		if (separation <= 0.0f)	{
			m->contacts[cp] = incidentFace[0];
			m->penetration = -separation;
			++cp;
		}
		else
			m->penetration = 0;

		separation = dot(refFaceNormal, incidentFace[1]) - refC;
		if (separation <= 0.0f)	{
			m->contacts[cp] = incidentFace[1];

			m->penetration += -separation;
			++cp;

			m->penetration /= (float)cp;
		}

		m->contactCount = cp;
	}
コード例 #2
0
ファイル: TetraCollision.cpp プロジェクト: tedsta/Tetraverse
void gridToPolygon(phys::Collision* c, phys::RigidBody *a, phys::RigidBody *b)
{
    c->addManifold();
    c->getLastManifold()->contactCount = 0;

    GridShape* grid = reinterpret_cast<GridShape*>(a->getShape());
    phys::PolygonShape* poly = reinterpret_cast<phys::PolygonShape*>(b->getShape());

    sf::Vector2f topLeft(FLT_MAX, FLT_MAX);
    sf::Vector2f botRight(-FLT_MAX, -FLT_MAX);

    for (unsigned int i = 0; i < poly->getVertices().size(); i++)
    {
        // Retrieve a face normal from A
        sf::Vector2f n = poly->getNormals()[i];
        sf::Vector2f nw = poly->getU() * n;

        // Transform face normal into B's model space
        phys::Mat2 buT = grid->getU().transpose( );
        n = buT * nw;

        // Retrieve vertex on face from A, transform into
        // B's model space
        sf::Vector2f v = poly->getVertices()[i];
        v = (poly->getU() * v) + b->getPosition();
        v -= a->getPosition();
        v = buT * v;

        poly->transformedVertices[i] = v;
        poly->transformedNormals[i] = n;

        if (v.x < topLeft.x)
            topLeft.x = v.x;
        if (v.y < topLeft.y)
            topLeft.y = v.y;
        if (v.x > botRight.x)
            botRight.x = v.x;
        if (v.y > botRight.y)
            botRight.y = v.y;
    }

    int left = floor(topLeft.x/(PTU/TILE_SIZE))-1;
    int top = floor(topLeft.y/(PTU/TILE_SIZE))-1;
    int right = ceil(botRight.x/(PTU/TILE_SIZE))+1;
    int bot = ceil(botRight.y/(PTU/TILE_SIZE))+1;

    if (!grid->getGrid()->mWrapX)
    {
        left = std::max(left, 0);
        right = std::min(right, grid->getGrid()->mSizeX-1);
    }
	top = std::max(top, 0);
    bot = std::min(bot, grid->getGrid()->mSizeY-1);

    // Tile size
    float tileSize = PTU/TILE_SIZE;

    // Find the tile with the least overlap in the collision
    sf::Vector2f tileVerts[4];
    int tileVertCount;
    float tilePenetration = 0.f;
    float polyPenetration = 0.f;
    int tileFace;
    int polyFace;

    sf::Vector2f tileNormals[4];

    sf::Vector2f usedNormals[20];
    int usedNormalCount = 0;

    for (int y = top; y <= bot; y++)
    {
        for (int _x = left; _x <= right; _x++)
        {
            int x = _x;
            if (grid->getGrid()->mWrapX)
                x = grid->getGrid()->wrapX(x);

            if (!grid->getGrid()->mTiles[y][x].isSolid())
                continue;

            int left = x-1;
            int right = x+1;
            int up = y-1;
            int down = y+1;

            if (grid->getGrid()->mWrapX)
            {
                left = grid->getGrid()->wrapX(left);
                right = grid->getGrid()->wrapX(right);
            }

            bool leftT = false;
            bool rightT = false;
            bool upT = false;
            bool downT = false;

            if (left >= 0 && grid->getGrid()->mTiles[y][left].isSolid())
                leftT = true;
            if (right < grid->getGrid()->mSizeX && grid->getGrid()->mTiles[y][right].isSolid())
                rightT = true;
            if (up >= 0 && grid->getGrid()->mTiles[up][x].isSolid())
                upT = true;
            if (down < grid->getGrid()->mSizeY && grid->getGrid()->mTiles[down][x].isSolid())
                downT = true;

            sf::Vector2f start(_x*tileSize, y*tileSize);

            if (!leftT && !rightT && !upT && downT)
            {
                tileVertCount = 3;
                tileVerts[0] = start + sf::Vector2f(tileSize/2, 0);
                tileVerts[1] = start + sf::Vector2f(0, tileSize);
                tileVerts[2] = start + sf::Vector2f(tileSize, tileSize);

                tileNormals[0] = normalize(sf::Vector2f(-0.5, -1.f));
                tileNormals[1] = sf::Vector2f(0, 1);
                tileNormals[2] = normalize(sf::Vector2f(0.5, -1.f));
            }
            else if (!leftT && rightT && !upT && downT)
            {
                tileVertCount = 3;
                tileVerts[0] = start + sf::Vector2f(tileSize, 0);
                tileVerts[1] = start + sf::Vector2f(0, tileSize);
                tileVerts[2] = start + sf::Vector2f(tileSize, tileSize);

                tileNormals[0] = normalize(sf::Vector2f(-1.f, -1.f));
                tileNormals[1] = sf::Vector2f(0, 1);
                tileNormals[2] = sf::Vector2f(1.f, 0.f);
            }
            else if (leftT && !rightT && !upT && downT)
            {
                tileVertCount = 3;
                tileVerts[0] = start;
                tileVerts[1] = start + sf::Vector2f(0, tileSize);
                tileVerts[2] = start + sf::Vector2f(tileSize, tileSize);

                tileNormals[0] = sf::Vector2f(-1.f, 0.f);
                tileNormals[1] = sf::Vector2f(0, 1);
                tileNormals[2] = normalize(sf::Vector2f(1.f, -1.f));
            }
            else
            {
                tileVertCount = 4;
                tileVerts[0] = start;
                tileVerts[1] = start + sf::Vector2f(0, tileSize);
                tileVerts[2] = start + sf::Vector2f(tileSize, tileSize);
                tileVerts[3] = start + sf::Vector2f(tileSize, 0);

                tileNormals[0] = sf::Vector2f(-1, 0);
                tileNormals[1] = sf::Vector2f(0, 1);
                tileNormals[2] = sf::Vector2f(1, 0);
                tileNormals[3] = sf::Vector2f(0, -1);
            }


            // Check for a separating axis with A's face planes
            int faceA;
            float penetrationA = findAxisLeastPenetration(&faceA, poly->transformedVertices.data(),
                                                          poly->transformedNormals.data(), poly->transformedVertices.size(), tileVerts, tileVertCount);
            if(penetrationA >= 0.0f || phys::equal(penetrationA, 0.f))
                continue;

            // Check for a separating axis with B's face planes
            int faceB;
            float penetrationB = findAxisLeastPenetration(&faceB, tileVerts, tileNormals, tileVertCount,
                                                          poly->transformedVertices.data(), poly->transformedVertices.size());
            if(penetrationB >= 0.0f || phys::equal(penetrationB, 0.f))
                continue;

            tilePenetration = penetrationB;
            polyPenetration = penetrationA;
            tileFace = faceB;
            polyFace = faceA;

            sf::Uint32 referenceIndex;
            bool flip; // Always point from a to b

            phys::RigidBody* ref;
            sf::Vector2f* refVerts;
            sf::Vector2f* refNormals;
            int refCount;
            phys::RigidBody* inc;
            sf::Vector2f* incVerts;
            sf::Vector2f* incNormals;
            int incCount;

            // Determine which shape contains reference face
            if(phys::biasGreaterThan( tilePenetration, polyPenetration ))
            {
                ref = a;
                refVerts = tileVerts;
                refNormals = tileNormals;
                refCount = tileVertCount;

                inc = b;
                incVerts = poly->transformedVertices.data();
                incNormals = poly->transformedNormals.data();
                incCount = poly->transformedVertices.size();

                referenceIndex = tileFace;
                flip = false;
            }
            else
            {
                ref = b;
                refVerts = poly->transformedVertices.data();
                refNormals = poly->transformedNormals.data();
                refCount = poly->transformedVertices.size();

                inc = a;
                incVerts = tileVerts;
                incNormals = tileNormals;
                incCount = tileVertCount;

                referenceIndex = polyFace;
                flip = true;
            }

            bool used = false;
            for (int i = 0; i < usedNormalCount; i++)
            {
                if (refNormals[referenceIndex] == usedNormals[i])
                {
                    used = true;
                    break;
                }
            }

            if (used)
                continue;

            usedNormals[usedNormalCount] = refNormals[referenceIndex];
            usedNormalCount++;

            // World space incident face
            sf::Vector2f incidentFace[2];
            findIncidentFace(incidentFace, refVerts, refNormals, refCount, incVerts, incNormals, incCount, referenceIndex);

            //        y
            //        ^  ->n       ^
            //      +---c ------posPlane--
            //  x < | i |\
            //      +---+ c-----negPlane--
            //             \       v
            //              r
            //
            //  r : reference face
            //  i : incident poly
            //  c : clipped point
            //  n : incident normal

            // Setup reference face vertices
            sf::Vector2f v1 = refVerts[referenceIndex];
            referenceIndex = referenceIndex + 1 == refCount ? 0 : referenceIndex + 1;
            sf::Vector2f v2 = refVerts[referenceIndex];

            // Calculate reference face side normal in world space
            sf::Vector2f sidePlaneNormal = (v2 - v1);
            sidePlaneNormal = phys::normalize(sidePlaneNormal);

            // Orthogonalize
            sf::Vector2f refFaceNormal( sidePlaneNormal.y, -sidePlaneNormal.x );

            // ax + by = c
            // c is distance from origin
            float refC = phys::dot( refFaceNormal, v1 );
            float negSide = -phys::dot( sidePlaneNormal, v1 );
            float posSide =  phys::dot( sidePlaneNormal, v2 );

            // Clip incident face to reference face side planes
            if(clip( -sidePlaneNormal, negSide, incidentFace ) < 2)
                return; // Due to floating point error, possible to not have required points

            if(clip(  sidePlaneNormal, posSide, incidentFace ) < 2)
                return; // Due to floating point error, possible to not have required points

            // Flip
            c->getLastManifold()->normal = flip ? -refFaceNormal : refFaceNormal;

            // Keep points behind reference face
            sf::Uint32 cp = 0; // clipped points behind reference face
            float separation = phys::dot( refFaceNormal, incidentFace[0] ) - refC;
            if(separation <= 0.0f)
            {
                c->getLastManifold()->contacts[cp] = incidentFace[0];
                c->getLastManifold()->penetration = -separation;
                ++cp;
            }
            else
                c->getLastManifold()->penetration = 0;

            separation = phys::dot( refFaceNormal, incidentFace[1] ) - refC;
            if(separation <= 0.0f)
            {
                c->getLastManifold()->contacts[cp] = incidentFace[1];

                c->getLastManifold()->penetration += -separation;
                ++cp;

                // Average penetration
                c->getLastManifold()->penetration /= (float)cp;
            }

            c->getLastManifold()->contactCount = cp;

            c->addManifold();
            c->getLastManifold()->contactCount = 0;
        }
    }
}