← Back to branch summary

~choreonoid/choreonoid/debian

« back to all changes in this revision

Viewing changes to src/Collision/Opcode/OPC_RayAABBOverlap.h

  • Committer: Thomas Moulard
  • Date: 2012-10-23 12:43:24 UTC
  • Revision ID: git-v1:351cf736ad49bc7a9a7b9767dee760a013517a5d
Tags: upstream/1.1.0
Imported Upstream version 1.1.0

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/Collision/Opcode/OPC_RayAABBOverlap.h
 
1
// Opcode 1.1: ray-AABB overlap tests based on Woo's code
 
2
// Opcode 1.2: ray-AABB overlap tests based on the separating axis theorem
 
3
//
 
4
// The point of intersection is not computed anymore. The distance to impact is not needed anymore
 
5
// since we now have two different queries for segments or rays.
 
6
 
 
7
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
8
/**
 
9
 *      Computes a segment-AABB overlap test using the separating axis theorem. Segment is cached within the class.
 
10
 *      \param          center  [in] AABB center
 
11
 *      \param          extents [in] AABB extents
 
12
 *      \return         true on overlap
 
13
 */
 
14
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
15
inline_ BOOL RayCollider::SegmentAABBOverlap(const Point& center, const Point& extents)
 
16
{
 
17
        // Stats
 
18
        mNbRayBVTests++;
 
19
 
 
20
        float Dx = mData2.x - center.x;         if(fabsf(Dx) > extents.x + mFDir.x)     return FALSE;
 
21
        float Dy = mData2.y - center.y;         if(fabsf(Dy) > extents.y + mFDir.y)     return FALSE;
 
22
        float Dz = mData2.z - center.z;         if(fabsf(Dz) > extents.z + mFDir.z)     return FALSE;
 
23
 
 
24
        float f;
 
25
        f = mData.y * Dz - mData.z * Dy;        if(fabsf(f) > extents.y*mFDir.z + extents.z*mFDir.y)    return FALSE;
 
26
        f = mData.z * Dx - mData.x * Dz;        if(fabsf(f) > extents.x*mFDir.z + extents.z*mFDir.x)    return FALSE;
 
27
        f = mData.x * Dy - mData.y * Dx;        if(fabsf(f) > extents.x*mFDir.y + extents.y*mFDir.x)    return FALSE;
 
28
 
 
29
        return TRUE;
 
30
}
 
31
 
 
32
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
33
/**
 
34
 *      Computes a ray-AABB overlap test using the separating axis theorem. Ray is cached within the class.
 
35
 *      \param          center  [in] AABB center
 
36
 *      \param          extents [in] AABB extents
 
37
 *      \return         true on overlap
 
38
 */
 
39
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 
40
inline_ BOOL RayCollider::RayAABBOverlap(const Point& center, const Point& extents)
 
41
{
 
42
        // Stats
 
43
        mNbRayBVTests++;
 
44
 
 
45
//      float Dx = mOrigin.x - center.x;        if(fabsf(Dx) > extents.x && Dx*mDir.x>=0.0f)    return FALSE;
 
46
//      float Dy = mOrigin.y - center.y;        if(fabsf(Dy) > extents.y && Dy*mDir.y>=0.0f)    return FALSE;
 
47
//      float Dz = mOrigin.z - center.z;        if(fabsf(Dz) > extents.z && Dz*mDir.z>=0.0f)    return FALSE;
 
48
 
 
49
        float Dx = mOrigin.x - center.x;        if(GREATER(Dx, extents.x) && Dx*mDir.x>=0.0f)   return FALSE;
 
50
        float Dy = mOrigin.y - center.y;        if(GREATER(Dy, extents.y) && Dy*mDir.y>=0.0f)   return FALSE;
 
51
        float Dz = mOrigin.z - center.z;        if(GREATER(Dz, extents.z) && Dz*mDir.z>=0.0f)   return FALSE;
 
52
 
 
53
//      float Dx = mOrigin.x - center.x;        if(GREATER(Dx, extents.x) && ((SIR(Dx)-1)^SIR(mDir.x))>=0.0f)   return FALSE;
 
54
//      float Dy = mOrigin.y - center.y;        if(GREATER(Dy, extents.y) && ((SIR(Dy)-1)^SIR(mDir.y))>=0.0f)   return FALSE;
 
55
//      float Dz = mOrigin.z - center.z;        if(GREATER(Dz, extents.z) && ((SIR(Dz)-1)^SIR(mDir.z))>=0.0f)   return FALSE;
 
56
 
 
57
        float f;
 
58
        f = mDir.y * Dz - mDir.z * Dy;          if(fabsf(f) > extents.y*mFDir.z + extents.z*mFDir.y)    return FALSE;
 
59
        f = mDir.z * Dx - mDir.x * Dz;          if(fabsf(f) > extents.x*mFDir.z + extents.z*mFDir.x)    return FALSE;
 
60
        f = mDir.x * Dy - mDir.y * Dx;          if(fabsf(f) > extents.x*mFDir.y + extents.y*mFDir.x)    return FALSE;
 
61
 
 
62
        return TRUE;
 
63
}