void Physics::_sphereTriangleCollision(CollisionPacket* colPackage, const Vec3 &p1,const Vec3 &p2,const Vec3 &p3)
{
    // Make the plane containing this triangle.
    Plane trianglePlane(p1,p2,p3);

    // Is triangle front-facing to the velocity vector?
    // We only check front-facing triangles
    // (your choice of course)
    if (!trianglePlane.isFrontFacingTo(colPackage->velocity)) {
        std::cout << "not front facing. returning...\n";
        return;
    }

    // Get interval of plane intersection:
    Real t0, t1;
    bool embeddedInPlane = false;

    // Calculate the signed distance from sphere
    // position to triangle plane
    Real signedDistToTrianglePlane = trianglePlane.signedDistanceTo(colPackage->basePoint);

    // cache this as we’re going to use it a few times below:
    Real normalDotVelocity = glm::dot(trianglePlane.normal(),colPackage->velocity);

    // if sphere is travelling parrallel to the plane:
    if (normalDotVelocity == (Real)0.0) {
        if (std::fabs(signedDistToTrianglePlane) >= (Real)1.0) {
            // Sphere is not embedded in plane.
            // No collision possible:
            return;
        }
        else {
            // sphere is embedded in plane.
            // It intersects in the whole range [0..1]
            embeddedInPlane = true;
            t0 = (Real)0.0;
            t1 = (Real)1.0;
        }
    }
    else {
        // N dot D is not 0. Calculate intersection interval:
        t0 = ((Real)-1.0 - signedDistToTrianglePlane) / normalDotVelocity;
        t1 = ((Real)1.0 - signedDistToTrianglePlane) / normalDotVelocity;

        // Swap so t0 < t1
        if (t0 > t1) {
            Real temp = t1;
            t1 = t0;
            t0 = temp;
        }

        // Check that at least one result is within range:
        if (t0 > (Real)1.0 || t1 < (Real)0.0) {
            // Both t values are outside values [0,1]
            // No collision possible:
            return;
        }

        // Clamp to [0,1]
        if (t0 < (Real)0.0) t0 = (Real)0.0;
        if (t1 < (Real)0.0) t1 = (Real)0.0;
        if (t0 > (Real)1.0) t0 = (Real)1.0;
        if (t1 > (Real)1.0) t1 = (Real)1.0;
    }

    // OK, at this point we have two time values t0 and t1
    // between which the swept sphere intersects with the
    // triangle plane. If any collision is to occur it must
    // happen within this interval.
    Vec3 collisionPoint;
    bool foundCollison = false;
    Real t = (Real)1.0;

    // First we check for the easy case - collision inside
    // the triangle. If this happens it must be at time t0
    // as this is when the sphere rests on the front side
    // of the triangle plane. Note, this can only happen if
    // the sphere is not embedded in the triangle plane.

    if (!embeddedInPlane) {
        Vec3 planeIntersectionPoint = (colPackage->basePoint - trianglePlane.normal()) + t0 * colPackage->velocity;
        if( checkPointInTriangle2((glm::vec3)planeIntersectionPoint, (glm::vec3)p1, (glm::vec3)p2, (glm::vec3)p3) ) //can use checkPointInTriangle2 as well.
        {
            foundCollison = true;
            t = t0;
            collisionPoint = planeIntersectionPoint;
            //std::cout << "collision inside the triangle!\n";
        }
    }

    // if we haven’t found a collision already we’ll have to
    // sweep sphere against points and edges of the triangle.
    // Note: A collision inside the triangle (the check above)
    // will always happen before a vertex or edge collision!
    // This is why we can skip the swept test if the above
    // gives a collision!
    if (!foundCollison) {
        // some commonly used terms:
        Vec3 velocity = colPackage->velocity;
        Vec3 base = colPackage->basePoint;
        Real velocitySquaredLength = glm::squaredLength(velocity);
        Real a,b,c; // Params for equation
        Real newT;

        // For each vertex or edge a quadratic equation have to
        // be solved. We parameterize this equation as
        // a*t^2 + b*t + c = 0 and below we calculate the
        // parameters a,b and c for each test.
        // Check against points:
        a = velocitySquaredLength;

        // P1
        b = (Real)2.0*(glm::dot(velocity,base-p1));
        c = glm::squaredLength(p1-base) - (Real)1.0;
        if (getLowestRoot(a,b,c, t, &newT)) {
            t = newT;
            foundCollison = true;
            collisionPoint = p1;
        }

        // P2
        b = (Real)2.0*(glm::dot(velocity,base-p2));
        c = glm::squaredLength(p2-base) - (Real)1.0;
        if (getLowestRoot(a,b,c, t, &newT)) {
            t = newT;
            foundCollison = true;
            collisionPoint = p2;
        }

        // P3
        b = (Real)2.0*(glm::dot(velocity,base-p3));
        c = glm::squaredLength(p3-base) - (Real)1.0;
        if (getLowestRoot(a,b,c, t, &newT)) {
            t = newT;
            foundCollison = true;
            collisionPoint = p3;
        }

        // Check agains edges:
        // p1 -> p2:
        Vec3 edge = p2-p1;
        Vec3 baseToVertex = p1 - base;
        Real edgeSquaredLength = glm::squaredLength(edge);
        Real edgeDotVelocity = glm::dot(edge,velocity);
        Real edgeDotBaseToVertex = glm::dot(edge,baseToVertex);

        // Calculate parameters for equation
        a = edgeSquaredLength * (-velocitySquaredLength) + edgeDotVelocity * edgeDotVelocity;
        b = edgeSquaredLength * ((Real)2.0 * glm::dot(velocity,baseToVertex)) - (Real)2.0*edgeDotVelocity*edgeDotBaseToVertex;
        c = edgeSquaredLength * ((Real)1.0 - glm::squaredLength(baseToVertex)) + edgeDotBaseToVertex*edgeDotBaseToVertex;

        // Does the swept sphere collide against infinite edge?
        if (getLowestRoot(a,b,c, t, &newT)) {
            // Check if intersection is within line segment:
            Real f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
            if (f >= (Real)0.0 && f <= (Real)1.0) {
                // intersection took place within segment.
                t = newT;
                foundCollison = true;
                collisionPoint = p1 + f*edge;
            }
        }

        // p2 -> p3:
        edge = p3-p2;
        baseToVertex = p2 - base;
        edgeSquaredLength = glm::squaredLength(edge);
        edgeDotVelocity = glm::dot(edge,velocity);
        edgeDotBaseToVertex = glm::dot(edge,baseToVertex);
        a = edgeSquaredLength * (-velocitySquaredLength) + edgeDotVelocity * edgeDotVelocity;
        b = edgeSquaredLength * ((Real)2.0 * glm::dot(velocity,baseToVertex)) - (Real)2.0 * edgeDotVelocity * edgeDotBaseToVertex;
        c = edgeSquaredLength * ((Real)1.0 - glm::squaredLength(baseToVertex)) + edgeDotBaseToVertex * edgeDotBaseToVertex;
        if (getLowestRoot(a,b,c, t, &newT)) {
            Real f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
            if (f >= (Real)0.0 && f <= (Real)1.0) {
                t = newT;
                foundCollison = true;
                collisionPoint = p2 + f*edge;
            }
        }

        // p3 -> p1:
        edge = p1-p3;
        baseToVertex = p3 - base;
        edgeSquaredLength = glm::squaredLength(edge);
        edgeDotVelocity = glm::dot(edge,velocity);

        edgeDotBaseToVertex = glm::dot(edge,baseToVertex);
        a = edgeSquaredLength * (-velocitySquaredLength) + edgeDotVelocity * edgeDotVelocity;
        b = edgeSquaredLength * ((Real)2.0 * glm::dot(velocity,baseToVertex)) - (Real)2.0 * edgeDotVelocity * edgeDotBaseToVertex;
        c = edgeSquaredLength * ((Real)1.0 - glm::squaredLength(baseToVertex)) + edgeDotBaseToVertex * edgeDotBaseToVertex;
        if (getLowestRoot(a,b,c, t, &newT)) {
            Real f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength;
            if (f >= (Real)0.0 && f <= (Real)1.0) {
                t = newT;
                foundCollison = true;
                collisionPoint = p3 + f*edge;
            }
        }
    }

    // Set result:
    if (foundCollison == true) {
        // distance to collision: ’t’ is time of collision
        Real distToCollision = t * glm::length(colPackage->velocity);

        // Does this triangle qualify for the closest hit?
        // it does if it’s the first hit or the closest
        if (colPackage->foundCollision == false || distToCollision < colPackage->nearestDistance) {
            // Collision information nessesary for sliding
            colPackage->nearestDistance = distToCollision;
            colPackage->intersectionPoint = collisionPoint;
            colPackage->foundCollision = true;
        }
    }
}

void solveCollision(CollisionPacket *colData)
{
    //slide plane from point and normal
    Plane slidePlane(colData->intersectionPoint, colData->basePoint - colData->intersectionPoint);

    //destination point as if there was no collision response
    Vec3 destinationPoint = colData->basePoint + colData->velocity;

    //new destination point
    Vec3 newDestinationPoint = destinationPoint - slidePlane.signedDistanceTo(destinationPoint) * slidePlane.normal();

    Vec3 intersectionToNewDestination = newDestinationPoint - colData->intersectionPoint;
    colData->velocity = colData->velocity * colData->nearestDistance + intersectionToNewDestination;
}