Reputation: 21
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