kmBool kmRay3IntersectTriangle(const kmRay3* ray, const kmVec3* v0, const kmVec3* v1, const kmVec3* v2, kmVec3* intersection, kmVec3* normal, kmScalar* distance) {
kmVec3 e1, e2, pvec, tvec, qvec, dir;
kmScalar det, inv_det, u, v, t;
kmVec3Normalize(&dir, &ray->dir);
kmVec3Subtract(&e1, v1, v0);
kmVec3Subtract(&e2, v2, v0);
kmVec3Cross(&pvec, &dir, &e2);
det = kmVec3Dot(&e1, &pvec);
/* Backfacing, discard. */
if(det < kmEpsilon) {
return KM_FALSE;
}
if(kmAlmostEqual(det, 0)) {
return KM_FALSE;
}
inv_det = 1.0 / det;
kmVec3Subtract(&tvec, &ray->start, v0);
u = inv_det * kmVec3Dot(&tvec, &pvec);
if(u < 0.0 || u > 1.0) {
return KM_FALSE;
}
kmVec3Cross(&qvec, &tvec, &e1);
v = inv_det * kmVec3Dot(&dir, &qvec);
if(v < 0.0 || (u + v) > 1.0) {
return KM_FALSE;
}
t = inv_det * kmVec3Dot(&e2, &qvec);
if(t > kmEpsilon && (t*t) <= kmVec3LengthSq(&ray->dir)) {
kmVec3 scaled;
*distance = t; /* Distance */
kmVec3Cross(normal, &e1, &e2); /* Surface normal of collision */
kmVec3Normalize(normal, normal);
kmVec3Normalize(&scaled, &dir);
kmVec3Scale(&scaled, &scaled, *distance);
kmVec3Add(intersection, &ray->start, &scaled);
return KM_TRUE;
}
return KM_FALSE;
}
/**
* Creates a plane from 3 points. The result is stored in pOut.
* pOut is returned.
*/
kmPlane* const kmPlaneFromPoints(kmPlane* pOut, const kmVec3* p1, const kmVec3* p2, const kmVec3* p3)
{
/*
v = (B − A) × (C − A)
n = 1⁄|v| v
Outa = nx
Outb = ny
Outc = nz
Outd = −n⋅A
*/
kmVec3 n, v1, v2;
kmVec3Subtract(&v1, p2, p1); //Create the vectors for the 2 sides of the triangle
kmVec3Subtract(&v2, p3, p1);
kmVec3Cross(&n, &v1, &v2); //Use the cross product to get the normal
kmVec3Normalize(&n, &n); //Normalize it and assign to pOut->m_N
pOut->a = n.x;
pOut->b = n.y;
pOut->c = n.z;
pOut->d = kmVec3Dot(kmVec3Scale(&n, &n, -1.0), p1);
return pOut;
}
kmVec3* kmVec3ProjectOnToVec3(const kmVec3* pIn, const kmVec3* other, kmVec3* projection) {
kmScalar scale = kmVec3Length(pIn) * kmVec3Dot(pIn, other);
kmVec3Normalize(projection, other);
kmVec3Scale(projection, projection, scale);
return projection;
}
/**
* Reflects a vector about a given surface normal. The surface normal is
* assumed to be of unit length.
*/
kmVec3* kmVec3Reflect(kmVec3* pOut, const kmVec3* pIn, const kmVec3* normal) {
kmVec3 tmp;
kmVec3Scale(&tmp, normal, 2.0f * kmVec3Dot(pIn, normal));
kmVec3Subtract(pOut, pIn, &tmp);
return pOut;
}
kmVec3* kmPlaneGetIntersection(kmVec3* pOut, const kmPlane* p1, const kmPlane* p2, const kmPlane* p3) {
kmVec3 n1, n2, n3, cross;
kmVec3 r1, r2, r3;
double denom = 0;
kmVec3Fill(&n1, p1->a, p1->b, p1->c);
kmVec3Fill(&n2, p2->a, p2->b, p2->c);
kmVec3Fill(&n3, p3->a, p3->b, p3->c);
kmVec3Cross(&cross, &n2, &n3);
denom = kmVec3Dot(&n1, &cross);
if (kmAlmostEqual(denom, 0.0)) {
return NULL;
}
kmVec3Cross(&r1, &n2, &n3);
kmVec3Cross(&r2, &n3, &n1);
kmVec3Cross(&r3, &n1, &n2);
kmVec3Scale(&r1, &r1, -p1->d);
kmVec3Scale(&r2, &r2, p2->d);
kmVec3Scale(&r3, &r3, p3->d);
kmVec3Subtract(pOut, &r1, &r2);
kmVec3Subtract(pOut, pOut, &r3);
kmVec3Scale(pOut, pOut, 1.0 / denom);
/*p = -d1 * ( n2.Cross ( n3 ) ) – d2 * ( n3.Cross ( n1 ) ) – d3 * ( n1.Cross ( n2 ) ) / denom;*/
return pOut;
}
void kmVec3OrthoNormalize(kmVec3* normal, kmVec3* tangent) {
kmVec3 proj;
kmVec3Normalize(normal, normal);
kmVec3Scale(&proj, normal, kmVec3Dot(tangent, normal));
kmVec3Subtract(tangent, tangent, &proj);
kmVec3Normalize(tangent, tangent);
}
kmVec3* kmVec3ProjectOnToPlane(kmVec3* pOut, const kmVec3* point, const struct kmPlane* plane) {
kmVec3 N;
kmVec3Fill(&N, plane->a, plane->b, plane->c);
kmVec3Normalize(&N, &N);
kmScalar distance = -kmVec3Dot(&N, point);
kmVec3Scale(&N, &N, distance);
kmVec3Add(pOut, point, &N);
return pOut;
}
kmVec3* kmVec3Reflect(kmVec3* pOut, const kmVec3* v1, const kmVec3* normal) {
const kmScalar d = 2 * kmVec3Dot(v1, normal);
kmVec3Fill(pOut,
v1->x - d * normal->x,
v1->y - d * normal->y,
v1->z - d * normal->z);
return pOut;
}
kmVec3* kmVec3ProjectOnToVec3(const kmVec3* w, const kmVec3* v,
kmVec3* projection) {
kmVec3 unitW, unitV;
kmVec3Normalize(&unitW, w);
kmVec3Normalize(&unitV, v);
kmScalar cos0 = kmVec3Dot(&unitW, &unitV);
kmVec3Scale(projection, &unitV, kmVec3Length(w) * cos0);
return projection;
}
/**
* Builds a translation matrix in the same way as gluLookAt()
* the resulting matrix is stored in pOut. pOut is returned.
*/
kmMat4* kmMat4LookAt(kmMat4* pOut, const kmVec3* pEye,
const kmVec3* pCenter, const kmVec3* pUp)
{
kmVec3 f;
kmVec3 s;
kmVec3 u;
kmVec3Subtract(&f, pCenter, pEye);
kmVec3Normalize(&f, &f);
kmVec3Cross(&s, &f, pUp);
kmVec3Normalize(&s, &s);
kmVec3Cross(&u, &s, &f);
pOut->mat[0] = s.x;
pOut->mat[1] = u.x;
pOut->mat[2] = -f.x;
pOut->mat[3] = 0.0;
pOut->mat[4] = s.y;
pOut->mat[5] = u.y;
pOut->mat[6] = -f.y;
pOut->mat[7] = 0.0;
pOut->mat[8] = s.z;
pOut->mat[9] = u.z;
pOut->mat[10] = -f.z;
pOut->mat[11] = 0.0;
pOut->mat[12] = -kmVec3Dot(&s, pEye);
pOut->mat[13] = -kmVec3Dot(&u, pEye);
pOut->mat[14] = kmVec3Dot(&f, pEye);
pOut->mat[15] = 1.0;
return pOut;
}
/*
* Returns a Quaternion representing the angle between two vectors
*/
kmQuaternion* kmQuaternionBetweenVec3(kmQuaternion* pOut, const kmVec3* u, const kmVec3* v) {
kmVec3 w;
kmScalar len;
kmQuaternion q;
if(kmVec3AreEqual(u, v)) {
kmQuaternionIdentity(pOut);
return pOut;
}
len = sqrtf(kmVec3LengthSq(u) * kmVec3LengthSq(v));
kmVec3Cross(&w, u, v);
kmQuaternionFill(&q, w.x, w.y, w.z, kmVec3Dot(u, v) + len);
return kmQuaternionNormalize(pOut, &q);
}
kmPlane* const kmPlaneFromPointNormal(kmPlane* pOut, const kmVec3* pPoint, const kmVec3* pNormal)
{
/*
Planea = Nx
Planeb = Ny
Planec = Nz
Planed = −N⋅P
*/
pOut->a = pNormal->x;
pOut->b = pNormal->y;
pOut->c = pNormal->z;
pOut->d = -kmVec3Dot(pNormal, pPoint);
return pOut;
}
static inline bool
_frustum_contains_sphere(
ne_frustum *f,
ne_bounding_sphere *s)
{
float dist = 0.f;
float rad = -s->radius;
for (uint8_t i = 0; i < 6; ++i) {
dist = kmVec3Dot(&f->planes[i].normal, &s->center)
+ f->planes[i].distance;
if (dist < rad)
return false;
}
return true;
}
kmQuaternion* kmQuaternionExtractRotationAroundAxis(const kmQuaternion* pIn, const kmVec3* axis, kmQuaternion* pOut) {
/**
Given a quaternion, and an axis. This extracts the rotation around the axis into pOut as another quaternion.
Uses the swing-twist decomposition.
http://stackoverflow.com/questions/3684269/component-of-a-quaternion-rotation-around-an-axis/22401169?noredirect=1#comment34098058_22401169
*/
kmVec3 ra;
kmScalar d;
kmVec3Fill(&ra, pIn->x, pIn->y, pIn->z);
d = kmVec3Dot(&ra, axis);
kmQuaternionFill(pOut, axis->x * d, axis->y * d, axis->z * d, pIn->w);
kmQuaternionNormalize(pOut, pOut);
return pOut;
}
/* Calculate view matrix */
static void dlCameraCalculateViewMatrix( dlCamera *object )
{
kmVec3 tgtv, upvector;
kmScalar dp;
CALL("%p", object);
kmVec3Subtract( &tgtv, &object->target, &object->translation );
kmVec3Normalize( &tgtv, &tgtv );
kmVec3Normalize( &upvector, &object->upVector );
dp = kmVec3Dot( &tgtv, &upvector );
if( dp == 1.f )
{
upvector.x += 0.5f;
}
kmMat4LookAt( &object->view, &object->translation, &object->target, &upvector );
/* Frustum recalculation here */
kmMat4Multiply( &object->matrix, &object->projection, &object->view );
}
kmQuaternion* kmQuaternionRotationBetweenVec3(kmQuaternion* pOut, const kmVec3* vec1, const kmVec3* vec2, const kmVec3* fallback) {
kmVec3 v1, v2;
kmScalar a;
kmVec3Assign(&v1, vec1);
kmVec3Assign(&v2, vec2);
kmVec3Normalize(&v1, &v1);
kmVec3Normalize(&v2, &v2);
a = kmVec3Dot(&v1, &v2);
if (a >= 1.0) {
kmQuaternionIdentity(pOut);
return pOut;
}
if (a < (1e-6f - 1.0f)) {
if (fabs(kmVec3LengthSq(fallback)) < kmEpsilon) {
kmQuaternionRotationAxisAngle(pOut, fallback, kmPI);
} else {
kmVec3 axis;
kmVec3 X;
X.x = 1.0;
X.y = 0.0;
X.z = 0.0;
kmVec3Cross(&axis, &X, vec1);
/*If axis is zero*/
if (fabs(kmVec3LengthSq(&axis)) < kmEpsilon) {
kmVec3 Y;
Y.x = 0.0;
Y.y = 1.0;
Y.z = 0.0;
kmVec3Cross(&axis, &Y, vec1);
}
kmVec3Normalize(&axis, &axis);
kmQuaternionRotationAxisAngle(pOut, &axis, kmPI);
}
} else {
kmScalar s = sqrtf((1+a) * 2);
kmScalar invs = 1 / s;
kmVec3 c;
kmVec3Cross(&c, &v1, &v2);
pOut->x = c.x * invs;
pOut->y = c.y * invs;
pOut->z = c.z * invs;
pOut->w = s * 0.5f;
kmQuaternionNormalize(pOut, pOut);
}
return pOut;
}
