Dimas
Dimas

Reputation: 21

Invalid RayCast algorithm output

I am writing my own physics engine in C++ using the glm library. But when I wrote a RayCast function that should detect the intersection of a ray with a triangle, I found that it gives wrong results. Here is the code of the class "Mesh" and "Rigid body"(For the test I made "vector vertices" == "vector hitBox"):

struct Mesh
{
    std::vector<glm::vec3> vertices;
    std::vector<uint32_t> indices;
    glm::dvec3 position;
    std::vector<glm::dvec3> hitBox;
    glm::dmat4 model;

    void setModel(const glm::dmat4 &model)
    {
        this->model = model;
    }
};

class RigidBody
{
public:
    static constexpr double EPS = 0.000001;

    Mesh mesh;

    std::pair<glm::dvec3, double> planeIntersection(const glm::dvec3 &start, const glm::dvec3 &end, const glm::dvec3 &point, const glm::dvec3 &normal) const
    {
        double sDot = glm::dot(start, normal);
        double k = (sDot - glm::dot(point, normal)) / (sDot - glm::dot(end, normal));
        glm::dvec3 res = start + (end - start) * k;
        return std::make_pair(res, k);
    }

    glm::dvec3 calculateNormal(const glm::dvec3 &point1, const glm::dvec3 &point2, const glm::dvec3 &point3) const
    {
        glm::dvec3 v1 = point2 - point1;
        glm::dvec3 v2 = point3 - point1;
        glm::dvec3 crossProduct = glm::cross(v1, v2);

        if (sqrAbs(crossProduct) > EPS)
            return glm::normalize(crossProduct);
        else
            return glm::dvec3(0.0);
    }

    bool isPointInsideTriangle(const glm::dvec3 &point, const glm::dvec3 &triangleNorm, const glm::dvec3 &point1, const glm::dvec3 &point2, const glm::dvec3 &point3) const
    {
        double dot1 = glm::dot(glm::normalize(glm::cross(point2 - point1, point - point1)), triangleNorm);
        double dot2 = glm::dot(glm::normalize(glm::cross(point3 - point2, point - point2)), triangleNorm);
        double dot3 = glm::dot(glm::normalize(glm::cross(point1 - point3, point - point3)), triangleNorm);

        if ((dot1 >= 0.0 && dot2 >= 0.0 && dot3 >= 0.0) || (dot1 <= 0.0 && dot2 <= 0.0 && dot3 <= 0.0))
            return true;

        return false;
    }

    std::pair<bool, glm::dvec3> rayCast(const glm::dvec3 &from, const glm::dvec3 &to)
    {
        glm::dvec3 v = glm::normalize(to - from);
        glm::dvec3 vModel = glm::dvec4(v, 1.0) * this->mesh.model;
        glm::dvec3 fromModel = glm::dvec4((from - mesh.position), 1.0) * this->mesh.model;
        glm::dvec3 toModel = glm::dvec4((to - mesh.position), 1.0) * this->mesh.model;

        for (size_t i = 0; i < mesh.indices.size() / 3; ++i)
        {
            auto normal = calculateNormal(mesh.hitBox[mesh.indices[i + 0]], mesh.hitBox[mesh.indices[i + 1]], mesh.hitBox[mesh.indices[i + 2]]);

            if (glm::dot(normal, vModel) > 0.0)
                continue;

            auto intersection = planeIntersection(fromModel, toModel, mesh.hitBox[mesh.indices[i + 0]], normal);

            if (intersection.second > 0.0 && isPointInsideTriangle(intersection.first, normal, mesh.hitBox[mesh.indices[i + 0]], mesh.hitBox[mesh.indices[i + 1]], mesh.hitBox[mesh.indices[i + 2]]))
                return std::make_pair(true, glm::dvec3(mesh.position));
        }

        return std::make_pair(false, glm::dvec3(0.0));
    }
};

Calculate front vector in class "Camera":

glm::dvec3 front;
front.x = cos(glm::radians(this->yaw)) * cos(glm::radians(this->pitch));
front.y = sin(glm::radians(this->pitch));
front.z = sin(glm::radians(this->yaw)) * cos(glm::radians(this->pitch));
this->front = glm::normalize(front);

Setting the model matrix and calling the rayCast function:

auto modelMatrix = glm::translate(glm::dmat4(1.0), glm::dvec3(0.0, 0.0, 0.0));
modelMatrix = glm::scale(modelMatrix, glm::dvec3(1.0, 1.5, 1.5));

box.mesh.setModel(modelMatrix);
isCollision = box.rayCast(camera.position, camera.position + camera.front).first;

Upvotes: 0

Views: 41

Answers (0)

Related Questions