/**
* Reflects a vector about a given surface normal. The surface normal is
* assumed to be of unit length.
*/
kmVec2* kmVec2Reflect(kmVec2* pOut, const kmVec2* pIn, const kmVec2* normal) {
kmVec2 tmp;
kmVec2Scale(&tmp, normal, 2.0f * kmVec2Dot(pIn, normal));
kmVec2Subtract(pOut, pIn, &tmp);
return pOut;
}
void calculate_line_normal(kmVec2 p1, kmVec2 p2, kmVec2 other_point, kmVec2* normal_out) {
/*
A = (3,4)
B = (2,1)
C = (1,3)
AB = (2,1) - (3,4) = (-1,-3)
AC = (1,3) - (3,4) = (-2,-1)
N = n(AB) = (-3,1)
D = dot(N,AC) = 6 + -1 = 5
since D > 0:
N = -N = (3,-1)
*/
kmVec2 edge, other_edge;
kmVec2Subtract(&edge, &p2, &p1);
kmVec2Subtract(&other_edge, &other_point, &p1);
kmVec2Normalize(&edge, &edge);
kmVec2Normalize(&other_edge, &other_edge);
kmVec2 n;
n.x = edge.y;
n.y = -edge.x;
kmScalar d = kmVec2Dot(&n, &other_edge);
if(d > 0.0f) {
n.x = -n.x;
n.y = -n.y;
}
normal_out->x = n.x;
normal_out->y = n.y;
kmVec2Normalize(normal_out, normal_out);
}
kmBool kmRay2IntersectBox(const kmRay2* ray, const kmVec2* p1, const kmVec2* p2, const kmVec2* p3, const kmVec2* p4,
kmVec2* intersection, kmVec2* normal_out) {
kmBool intersected = KM_FALSE;
kmVec2 intersect, final_intersect, normal;
kmScalar distance = 10000.0f;
const kmVec2* points[4];
points[0] = p1;
points[1] = p2;
points[2] = p3;
points[3] = p4;
unsigned int i = 0;
for(; i < 4; ++i) {
const kmVec2* this_point = points[i];
const kmVec2* next_point = (i == 3) ? points[0] : points[i+1];
const kmVec2* other_point = (i == 3 || i == 0) ? points[1] : points[0];
if(kmRay2IntersectLineSegment(ray, this_point, next_point, &intersect)) {
kmVec2 tmp;
kmScalar this_distance = kmVec2Length(kmVec2Subtract(&tmp, &intersect, &ray->start));
kmVec2 this_normal;
calculate_line_normal(*this_point, *next_point, *other_point, &this_normal);
if(this_distance < distance && kmVec2Dot(&this_normal, &ray->dir) < 0.0f) {
kmVec2Assign(&final_intersect, &intersect);
distance = this_distance;
intersected = KM_TRUE;
kmVec2Assign(&normal, &this_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;
}
/**
* Returns the angle in degrees between the two vectors
*/
kmScalar kmVec2DegreesBetween(const kmVec2* v1, const kmVec2* v2) {
if(kmVec2AreEqual(v1, v2)) {
return 0.0;
}
kmVec2 t1, t2;
kmVec2Normalize(&t1, v1);
kmVec2Normalize(&t2, v2);
kmScalar cross = kmVec2Cross(&t1, &t2);
kmScalar dot = kmVec2Dot(&t1, &t2);
/*
* acos is only defined for -1 to 1. Outside the range we
* get NaN even if that's just because of a floating point error
* so we clamp to the -1 - 1 range
*/
if(dot > 1.0) dot = 1.0;
if(dot < -1.0) dot = -1.0;
return kmRadiansToDegrees(atan2(cross, dot));
}
kmBool kmRay2IntersectTriangle(const kmRay2* ray, const kmVec2* p1, const kmVec2* p2, const kmVec2* p3, kmVec2* intersection, kmVec2* normal_out, kmScalar* distance_out) {
kmVec2 intersect;
kmVec2 final_intersect;
kmVec2 normal;
kmScalar distance = 10000.0f;
kmBool intersected = KM_FALSE;
if(kmRay2IntersectLineSegment(ray, p1, p2, &intersect)) {
kmVec2 tmp;
kmScalar this_distance = kmVec2Length(kmVec2Subtract(&tmp, &intersect, &ray->start));
kmVec2 this_normal;
calculate_line_normal(*p1, *p2, *p3, &this_normal);
if(this_distance < distance && kmVec2Dot(&this_normal, &ray->dir) < 0.0f) {
final_intersect.x = intersect.x;
final_intersect.y = intersect.y;
distance = this_distance;
kmVec2Assign(&normal, &this_normal);
intersected = KM_TRUE;
}
}
if(kmRay2IntersectLineSegment(ray, p2, p3, &intersect)) {
kmVec2 tmp;
kmScalar this_distance = kmVec2Length(kmVec2Subtract(&tmp, &intersect, &ray->start));
kmVec2 this_normal;
calculate_line_normal(*p2, *p3, *p1, &this_normal);
if(this_distance < distance && kmVec2Dot(&this_normal, &ray->dir) < 0.0f) {
final_intersect.x = intersect.x;
final_intersect.y = intersect.y;
distance = this_distance;
kmVec2Assign(&normal, &this_normal);
intersected = KM_TRUE;
}
}
if(kmRay2IntersectLineSegment(ray, p3, p1, &intersect)) {
kmVec2 tmp;
kmScalar this_distance = kmVec2Length(kmVec2Subtract(&tmp, &intersect, &ray->start));
kmVec2 this_normal;
calculate_line_normal(*p3, *p1, *p2, &this_normal);
if(this_distance < distance && kmVec2Dot(&this_normal, &ray->dir) < 0.0f) {
final_intersect.x = intersect.x;
final_intersect.y = intersect.y;
distance = this_distance;
kmVec2Assign(&normal, &this_normal);
intersected = KM_TRUE;
}
}
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;
}
if(distance) {
*distance_out = distance;
}
}
return intersected;
}
