int kmQuaternionAreEqual(const kmQuaternion* p1, const kmQuaternion* p2) {
if((!kmAlmostEqual(p1->x, p2->x)) || (!kmAlmostEqual(p1->y, p2->y)) || (!kmAlmostEqual(p1->z, p2->z)) || (!kmAlmostEqual(p1->w, p2->w))) {
return KM_FALSE;
}
return KM_TRUE;
}
/**
* Returns KM_TRUE if the 2 vectors are approximately equal
*/
kmBool kmVec3AreEqual(const kmVec3* p1, const kmVec3* p2)
{
if((!kmAlmostEqual(p1->x, p2->x)) || (!kmAlmostEqual(p1->y, p2->y)) || (!kmAlmostEqual(p1->z, p2->z))) {
return KM_FALSE;
}
return KM_TRUE;
}
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;
}
bool point_on_line(const kglt::Vec3& p, const kglt::Vec3& a, const kglt::Vec3& b) {
double ab = (b - a).length();
double ap = (p - a).length();
double pb = (b - p).length();
return kmAlmostEqual(ab, (ap + pb));
}
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;
}
/**
* Returns KM_TRUE if the 2 matrices are equal (approximately)
*/
int kmMat4AreEqual(const kmMat4* pMat1, const kmMat4* pMat2)
{
int i = 0;
assert(pMat1 != pMat2 && "You are comparing the same thing!");
for (i = 0; i < 16; ++i)
{
if(!kmAlmostEqual(pMat1->mat[i], pMat2->mat[i])) {
return KM_FALSE;
}
}
return KM_TRUE;
}
