axmol/cocos2dx/kazmath/src/ray2.c

187 lines
5.3 KiB
C

#include <assert.h>
#include <stdio.h>
#include "kazmath/ray2.h"
void kmRay2Fill(kmRay2* ray, kmScalar px, kmScalar py, kmScalar vx, kmScalar vy) {
ray->start.x = px;
ray->start.y = py;
ray->dir.x = vx;
ray->dir.y = vy;
}
kmBool kmRay2IntersectLineSegment(const kmRay2* ray, const kmVec2* p1, const kmVec2* p2, kmVec2* intersection) {
float x1 = ray->start.x;
float y1 = ray->start.y;
float x2 = ray->start.x + ray->dir.x;
float y2 = ray->start.y + ray->dir.y;
float x3 = p1->x;
float y3 = p1->y;
float x4 = p2->x;
float y4 = p2->y;
float denom = (y4 -y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
float ua, x, y;
//If denom is zero, the lines are parallel
if(denom > -kmEpsilon && denom < kmEpsilon) {
return KM_FALSE;
}
ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / denom;
// float ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / denom;
x = x1 + ua * (x2 - x1);
y = y1 + ua * (y2 - y1);
if(x < kmMin(p1->x, p2->x) - kmEpsilon ||
x > kmMax(p1->x, p2->x) + kmEpsilon ||
y < kmMin(p1->y, p2->y) - kmEpsilon ||
y > kmMax(p1->y, p2->y) + kmEpsilon) {
//Outside of line
//printf("Outside of line, %f %f (%f %f)(%f, %f)\n", x, y, p1->x, p1->y, p2->x, p2->y);
return KM_FALSE;
}
if(x < kmMin(x1, x2) - kmEpsilon ||
x > kmMax(x1, x2) + kmEpsilon ||
y < kmMin(y1, y2) - kmEpsilon ||
y > kmMax(y1, y2) + kmEpsilon) {
//printf("Outside of ray, %f %f (%f %f)(%f, %f)\n", x, y, x1, y1, x2, y2);
return KM_FALSE;
}
intersection->x = x;
intersection->y = y;
return KM_TRUE;
/*
kmScalar A1, B1, C1;
kmScalar A2, B2, C2;
A1 = ray->dir.y;
B1 = ray->dir.x;
C1 = A1 * ray->start.x + B1 * ray->start.y;
A2 = p2->y - p1->y;
B2 = p2->x - p1->x;
C2 = A2 * p1->x + B2 * p1->y;
double det = (A1 * B2) - (A2 * B1);
if(det == 0) {
printf("Parallel\n");
return KM_FALSE;
}
double x = (B2*C1 - B1*C2) / det;
double y = (A1*C2 - A2*C1) / det;
if(x < min(p1->x, p2->x) - kmEpsilon ||
x > max(p1->x, p2->x) + kmEpsilon ||
y < min(p1->y, p2->y) - kmEpsilon ||
y > max(p1->y, p2->y) + kmEpsilon) {
//Outside of line
printf("Outside of line, %f %f (%f %f)(%f, %f)\n", x, y, p1->x, p1->y, p2->x, p2->y);
return KM_FALSE;
}
kmScalar x1 = ray->start.x;
kmScalar x2 = ray->start.x + ray->dir.x;
kmScalar y1 = ray->start.y;
kmScalar y2 = ray->start.y + ray->dir.y;
if(x < min(x1, x2) - kmEpsilon ||
x > max(x1, x2) + kmEpsilon ||
y < min(y1, y2) - kmEpsilon ||
y > max(y1, y2) + kmEpsilon) {
printf("Outside of ray, %f %f (%f %f)(%f, %f)\n", x, y, x1, y1, x2, y2);
return KM_FALSE;
}
intersection->x = x;
intersection->y = y;
return KM_TRUE;*/
}
void calculate_line_normal(kmVec2 p1, kmVec2 p2, kmVec2* normal_out) {
kmVec2 tmp;
kmVec2Subtract(&tmp, &p2, &p1); //Get direction vector
normal_out->x = -tmp.y;
normal_out->y = tmp.x;
kmVec2Normalize(normal_out, normal_out);
//TODO: should check that the normal is pointing out of the triangle
}
kmBool kmRay2IntersectTriangle(const kmRay2* ray, const kmVec2* p1, const kmVec2* p2, const kmVec2* p3, kmVec2* intersection, kmVec2* normal_out) {
kmVec2 intersect;
kmVec2 final_intersect = {0., 0.}, normal = {0., 0.}; // Silencing LLVM SA.
kmScalar distance = 10000.0f;
kmBool intersected = KM_FALSE;
if(kmRay2IntersectLineSegment(ray, p1, p2, &intersect)) {
kmVec2 tmp;
kmScalar this_distance;
intersected = KM_TRUE;
this_distance = kmVec2Length(kmVec2Subtract(&tmp, &intersect, &ray->start));
if(this_distance < distance) {
final_intersect.x = intersect.x;
final_intersect.y = intersect.y;
distance = this_distance;
calculate_line_normal(*p1, *p2, &normal);
}
}
if(kmRay2IntersectLineSegment(ray, p2, p3, &intersect)) {
kmVec2 tmp;
kmScalar this_distance;
intersected = KM_TRUE;
this_distance = kmVec2Length(kmVec2Subtract(&tmp, &intersect, &ray->start));
if(this_distance < distance) {
final_intersect.x = intersect.x;
final_intersect.y = intersect.y;
distance = this_distance;
calculate_line_normal(*p2, *p3, &normal);
}
}
if(kmRay2IntersectLineSegment(ray, p3, p1, &intersect)) {
kmVec2 tmp;
kmScalar this_distance;
intersected = KM_TRUE;
this_distance = kmVec2Length(kmVec2Subtract(&tmp, &intersect, &ray->start));
if(this_distance < distance) {
final_intersect.x = intersect.x;
final_intersect.y = intersect.y;
//distance = this_distance;
calculate_line_normal(*p3, *p1, &normal);
}
}
if(intersected) {
intersection->x = final_intersect.x;
intersection->y = final_intersect.y;
if(normal_out) {
normal_out->x = normal.x;
normal_out->y = normal.y;
}
}
return intersected;
}
kmBool kmRay2IntersectCircle(const kmRay2* ray, const kmVec2 centre, const kmScalar radius, kmVec2* intersection) {
assert(0 && "Not implemented");
return 0;
}