bool IsEqualf(float A, float B)
{
if(Absf(A-B) <= EQUAL_EPSILON)
return true;
else
return false;
}
FDK_NS_BEGIN
// ::::: UTILITIES :::::
bool IsZerof(F32 val, F32 toll) {
if (Absf(val) < toll)
return true;
return false;
}
bool Get_Intersect(vect_t *linestart,vect_t *lineend,plan_t plan,vect_t * intersection, float *percentage)
{
vect_t direction,L1;
float linelength,dist_from_plane;
vect_t *vertex;
vect_t *normal;
vertex = &(plan.PointOnPlane);
normal = &(plan.VecteurNormal);
direction.X = lineend->X - linestart->X;
direction.Y = lineend->Y - linestart->Y;
direction.Z = lineend->Z - linestart->Z;
linelength=PdtScalaire(direction,*normal);
if(Absf(linelength)<EPSILON)
return false;
L1.X = vertex->X - linestart->X;
L1.Y = vertex->Y - linestart->Y;
L1.Z = vertex->Z - linestart->Z;
dist_from_plane = PdtScalaire(L1,*normal);
*percentage = dist_from_plane/linelength;
if(*percentage<0.0f)
return false;
else if(*percentage>1.0f)
return false;
else
{
intersection->X = linestart->X + direction.X*(*percentage);
intersection->Y = linestart->Y + direction.Y*(*percentage);
intersection->Z = linestart->Z + direction.Z*(*percentage);
}
return true;
}
//to do: try this...
bool IntersectRayOBB3f(const Ray3f& ray, const OBB3f& obb) {
//intersect ray-obb reference: http://www.geometrictools.com/LibMathematics/Intersection/Intersection.html intersect box ray
//bool IntrRay3Box3<Real>::Test ()
//{
//Real WdU[3], AWdU[3], DdU[3], ADdU[3], AWxDdU[3], RHS;
F32 WdU[3], AWdU[3], DdU[3], ADdU[3], AWxDdU[3], RHS;
//Vector3<Real> diff = mRay->Origin - mBox->Center;
Vec3f diff;
Vec3fSub(ray.origin.point, obb.center.point, &diff);
//WdU[0] = mRay->Direction.Dot(mBox->Axis[0]);
WdU[0] = Vec3fDot(ray.direction.dir, obb.orientX);
//AWdU[0] = Math<Real>::FAbs(WdU[0]);
AWdU[0] = Absf(WdU[0]);
//DdU[0] = diff.Dot(mBox->Axis[0]);
DdU[0] = Vec3fDot(diff, obb.orientX);
//ADdU[0] = Math<Real>::FAbs(DdU[0]);
ADdU[0] = Absf(DdU[0]);
//if (ADdU[0] > mBox->Extent[0] && DdU[0]*WdU[0] >= (Real)0)
if ( ADdU[0] > obb.halfWidths.x && DdU[0]*WdU[0] >= 0.f) {
return false;
}
//WdU[1] = mRay->Direction.Dot(mBox->Axis[1]);
WdU[1] = Vec3fDot(ray.direction.dir, obb.orientY);
//AWdU[1] = Math<Real>::FAbs(WdU[1]);
AWdU[1] = Absf(WdU[1]);
//DdU[1] = diff.Dot(mBox->Axis[1]);
DdU[1] = Vec3fDot(diff, obb.orientY);
//ADdU[1] = Math<Real>::FAbs(DdU[1]);
ADdU[1] = Absf(DdU[1]);
//if (ADdU[1] > mBox->Extent[1] && DdU[1]*WdU[1] >= (Real)0)
if ( ADdU[1] > obb.halfWidths.y && DdU[1]*WdU[1] >= 0.f) {
return false;
}
//WdU[2] = mRay->Direction.Dot(mBox->Axis[2]);
WdU[2] = Vec3fDot(ray.direction.dir, obb.orientZ);
//AWdU[2] = Math<Real>::FAbs(WdU[2]);
AWdU[2] = Absf(WdU[2]);
//DdU[2] = diff.Dot(mBox->Axis[2]);
DdU[2] = Vec3fDot(diff, obb.orientZ);
//ADdU[2] = Math<Real>::FAbs(DdU[2]);
ADdU[2] = Absf(DdU[2]);
//if (ADdU[2] > mBox->Extent[2] && DdU[2]*WdU[2] >= (Real)0)
if ( ADdU[2] > obb.halfWidths.z && DdU[2]*WdU[2] >= 0.f) {
return false;
}
//Vector3<Real> WxD = mRay->Direction.Cross(diff);
Vec3f WxD;
Vec3fCross(ray.direction.dir, diff, &WxD);
//AWxDdU[0] = Math<Real>::FAbs(WxD.Dot(mBox->Axis[0]));
AWxDdU[0] = Absf(Vec3fDot(WxD, obb.orientX));
//RHS = mBox->Extent[1]*AWdU[2] + mBox->Extent[2]*AWdU[1];
RHS = obb.halfWidths.y * AWdU[2] + obb.halfWidths.z * AWdU[1];
if (AWxDdU[0] > RHS)
{
return false;
}
//AWxDdU[1] = Math<Real>::FAbs(WxD.Dot(mBox->Axis[1]));
AWxDdU[1] = Absf(Vec3fDot(WxD, obb.orientY));
//RHS = mBox->Extent[0]*AWdU[2] + mBox->Extent[2]*AWdU[0];
RHS = obb.halfWidths.x * AWdU[2] + obb.halfWidths.z * AWdU[0];
if (AWxDdU[1] > RHS)
{
return false;
}
//AWxDdU[2] = Math<Real>::FAbs(WxD.Dot(mBox->Axis[2]));
AWxDdU[2] = Absf(Vec3fDot(WxD, obb.orientZ));
//RHS = mBox->Extent[0]*AWdU[1] + mBox->Extent[1]*AWdU[0];
RHS = obb.halfWidths.x * AWdU[1] + obb.halfWidths.y * AWdU[0];
if (AWxDdU[2] > RHS)
{
return false;
}
return true;
}
bool IntersectAABBAABB3f(const Point3f& c1, const Vec3f& halfW1, const Point3f& c2, const Vec3f& halfW2) {
if ( Absf(c1.point.x - c2.point.x) > (halfW1.x + halfW2.x) ) return false;
if ( Absf(c1.point.y - c2.point.y) > (halfW1.y + halfW2.y) ) return false;
if ( Absf(c1.point.z - c2.point.z) > (halfW1.z + halfW2.z) ) return false;
return true;
}
void Vec3fAbs (const Vec3f& inVec3f, Vec3f* outVec) {
outVec->x = Absf(inVec3f.x);
outVec->y = Absf(inVec3f.y);
outVec->z = Absf(inVec3f.z);
}
void Vec2fAbs (const Vec2f& inVec2f, Vec2f* outVec) {
outVec->x = Absf(inVec2f.x);
outVec->y = Absf(inVec2f.y);
}
