IceOBB.cpp
Go to the documentation of this file.
00001 
00002 
00008 
00009 
00011 
00017 
00018 
00020 // Precompiled Header
00021 #include "Stdafx.h"
00022 
00023 using namespace IceMaths;
00024 
00026 
00031 
00032 bool OBB::ContainsPoint(const Point& p) const
00033 {
00034         // Point in OBB test using lazy evaluation and early exits
00035 
00036         // Translate to box space
00037         Point RelPoint = p - mCenter;
00038 
00039         // Point * mRot maps from box space to world space
00040         // mRot * Point maps from world space to box space (what we need here)
00041 
00042         float f = mRot.m[0][0] * RelPoint.x + mRot.m[0][1] * RelPoint.y + mRot.m[0][2] * RelPoint.z;
00043         if(f >= mExtents.x || f <= -mExtents.x) return false;
00044 
00045         f = mRot.m[1][0] * RelPoint.x + mRot.m[1][1] * RelPoint.y + mRot.m[1][2] * RelPoint.z;
00046         if(f >= mExtents.y || f <= -mExtents.y) return false;
00047 
00048         f = mRot.m[2][0] * RelPoint.x + mRot.m[2][1] * RelPoint.y + mRot.m[2][2] * RelPoint.z;
00049         if(f >= mExtents.z || f <= -mExtents.z) return false;
00050         return true;
00051 }
00052 
00054 
00059 
00060 void OBB::Create(const AABB& aabb, const Matrix4x4& mat)
00061 {
00062         // Note: must be coherent with Rotate()
00063 
00064         aabb.GetCenter(mCenter);
00065         aabb.GetExtents(mExtents);
00066         // Here we have the same as OBB::Rotate(mat) where the obb is (mCenter, mExtents, Identity).
00067 
00068         // So following what's done in Rotate:
00069         // - x-form the center
00070         mCenter *= mat;
00071         // - combine rotation with identity, i.e. just use given matrix
00072         mRot = mat;
00073 }
00074 
00076 
00081 
00082 bool OBB::ComputePlanes(Plane* planes)  const
00083 {
00084         // Checkings
00085         if(!planes)     return false;
00086 
00087         Point Axis0 = mRot[0];
00088         Point Axis1 = mRot[1];
00089         Point Axis2 = mRot[2];
00090 
00091         // Writes normals
00092         planes[0].n = Axis0;
00093         planes[1].n = -Axis0;
00094         planes[2].n = Axis1;
00095         planes[3].n = -Axis1;
00096         planes[4].n = Axis2;
00097         planes[5].n = -Axis2;
00098 
00099         // Compute a point on each plane
00100         Point p0 = mCenter + Axis0 * mExtents.x;
00101         Point p1 = mCenter - Axis0 * mExtents.x;
00102         Point p2 = mCenter + Axis1 * mExtents.y;
00103         Point p3 = mCenter - Axis1 * mExtents.y;
00104         Point p4 = mCenter + Axis2 * mExtents.z;
00105         Point p5 = mCenter - Axis2 * mExtents.z;
00106 
00107         // Compute d
00108         planes[0].d = -(planes[0].n|p0);
00109         planes[1].d = -(planes[1].n|p1);
00110         planes[2].d = -(planes[2].n|p2);
00111         planes[3].d = -(planes[3].n|p3);
00112         planes[4].d = -(planes[4].n|p4);
00113         planes[5].d = -(planes[5].n|p5);
00114 
00115         return true;
00116 }
00117 
00119 
00124 
00125 bool OBB::ComputePoints(Point* pts)     const
00126 {
00127         // Checkings
00128         if(!pts)        return false;
00129 
00130         Point Axis0 = mRot[0];
00131         Point Axis1 = mRot[1];
00132         Point Axis2 = mRot[2];
00133 
00134         Axis0 *= mExtents.x;
00135         Axis1 *= mExtents.y;
00136         Axis2 *= mExtents.z;
00137 
00138         //     7+------+6                       0 = ---
00139         //     /|     /|                        1 = +--
00140         //    / |    / |                        2 = ++-
00141         //   / 4+---/--+5                       3 = -+-
00142         // 3+------+2 /    y   z        4 = --+
00143         //  | /    | /     |  /         5 = +-+
00144         //  |/     |/      |/           6 = +++
00145         // 0+------+1      *---x        7 = -++
00146 
00147         pts[0] = mCenter - Axis0 - Axis1 - Axis2;
00148         pts[1] = mCenter + Axis0 - Axis1 - Axis2;
00149         pts[2] = mCenter + Axis0 + Axis1 - Axis2;
00150         pts[3] = mCenter - Axis0 + Axis1 - Axis2;
00151         pts[4] = mCenter - Axis0 - Axis1 + Axis2;
00152         pts[5] = mCenter + Axis0 - Axis1 + Axis2;
00153         pts[6] = mCenter + Axis0 + Axis1 + Axis2;
00154         pts[7] = mCenter - Axis0 + Axis1 + Axis2;
00155 
00156         return true;
00157 }
00158 
00160 
00165 
00166 bool OBB::ComputeVertexNormals(Point* pts)      const
00167 {
00168         static float VertexNormals[] = 
00169         {
00170                 -INVSQRT3,      -INVSQRT3,      -INVSQRT3,
00171                 INVSQRT3,       -INVSQRT3,      -INVSQRT3,
00172                 INVSQRT3,       INVSQRT3,       -INVSQRT3,
00173                 -INVSQRT3,      INVSQRT3,       -INVSQRT3,
00174                 -INVSQRT3,      -INVSQRT3,      INVSQRT3,
00175                 INVSQRT3,       -INVSQRT3,      INVSQRT3,
00176                 INVSQRT3,       INVSQRT3,       INVSQRT3,
00177                 -INVSQRT3,      INVSQRT3,       INVSQRT3
00178         };
00179 
00180         if(!pts)        return false;
00181 
00182         const Point* VN = (const Point*)VertexNormals;
00183         for(udword i=0;i<8;i++)
00184         {
00185                 pts[i] = VN[i] * mRot;
00186         }
00187 
00188         return true;
00189 }
00190 
00192 
00196 
00197 const udword* OBB::GetEdges() const
00198 {
00199         static udword Indices[] = {
00200         0, 1,   1, 2,   2, 3,   3, 0,
00201         7, 6,   6, 5,   5, 4,   4, 7,
00202         1, 5,   6, 2,
00203         3, 7,   4, 0
00204         };
00205         return Indices;
00206 }
00207 
00209 
00213 
00214 const Point* OBB::GetLocalEdgeNormals() const
00215 {
00216         static float EdgeNormals[] = 
00217         {
00218                 0,                      -INVSQRT2,      -INVSQRT2,      // 0-1
00219                 INVSQRT2,       0,                      -INVSQRT2,      // 1-2
00220                 0,                      INVSQRT2,       -INVSQRT2,      // 2-3
00221                 -INVSQRT2,      0,                      -INVSQRT2,      // 3-0
00222 
00223                 0,                      INVSQRT2,       INVSQRT2,       // 7-6
00224                 INVSQRT2,       0,                      INVSQRT2,       // 6-5
00225                 0,                      -INVSQRT2,      INVSQRT2,       // 5-4
00226                 -INVSQRT2,      0,                      INVSQRT2,       // 4-7
00227 
00228                 INVSQRT2,       -INVSQRT2,      0,                      // 1-5
00229                 INVSQRT2,       INVSQRT2,       0,                      // 6-2
00230                 -INVSQRT2,      INVSQRT2,       0,                      // 3-7
00231                 -INVSQRT2,      -INVSQRT2,      0                       // 4-0
00232         };
00233         return (const Point*)EdgeNormals;
00234 }
00235 
00237 
00242 
00243 void OBB::ComputeWorldEdgeNormal(udword edge_index, Point& world_normal) const
00244 {
00245         ASSERT(edge_index<12);
00246         world_normal = GetLocalEdgeNormals()[edge_index] * mRot;
00247 }
00248 
00250 
00254 
00255 void OBB::ComputeLSS(LSS& lss) const
00256 {
00257         Point Axis0 = mRot[0];
00258         Point Axis1 = mRot[1];
00259         Point Axis2 = mRot[2];
00260 
00261         switch(mExtents.LargestAxis())
00262         {
00263                 case 0:
00264                         lss.mRadius = (mExtents.y + mExtents.z)*0.5f;
00265                         lss.mP0 = mCenter + Axis0 * (mExtents.x - lss.mRadius);
00266                         lss.mP1 = mCenter - Axis0 * (mExtents.x - lss.mRadius);
00267                         break;
00268                 case 1:
00269                         lss.mRadius = (mExtents.x + mExtents.z)*0.5f;
00270                         lss.mP0 = mCenter + Axis1 * (mExtents.y - lss.mRadius);
00271                         lss.mP1 = mCenter - Axis1 * (mExtents.y - lss.mRadius);
00272                         break;
00273                 case 2:
00274                         lss.mRadius = (mExtents.x + mExtents.y)*0.5f;
00275                         lss.mP0 = mCenter + Axis2 * (mExtents.z - lss.mRadius);
00276                         lss.mP1 = mCenter - Axis2 * (mExtents.z - lss.mRadius);
00277                         break;
00278         }
00279 }
00280 
00282 
00287 
00288 BOOL OBB::IsInside(const OBB& box) const
00289 {
00290         // Make a 4x4 from the box & inverse it
00291         Matrix4x4 M0Inv;
00292         {
00293                 Matrix4x4 M0 = box.mRot;
00294                 M0.SetTrans(box.mCenter);
00295                 InvertPRMatrix(M0Inv, M0);
00296         }
00297 
00298         // With our inversed 4x4, create box1 in space of box0
00299         OBB _1in0;
00300         Rotate(M0Inv, _1in0);
00301 
00302         // This should cancel out box0's rotation, i.e. it's now an AABB.
00303         // => Center(0,0,0), Rot(identity)
00304 
00305         // The two boxes are in the same space so now we can compare them.
00306 
00307         // Create the AABB of (box1 in space of box0)
00308         const Matrix3x3& mtx = _1in0.mRot;
00309 
00310         float f = fabsf(mtx.m[0][0] * mExtents.x) + fabsf(mtx.m[1][0] * mExtents.y) + fabsf(mtx.m[2][0] * mExtents.z) - box.mExtents.x;
00311         if(f > _1in0.mCenter.x)         return FALSE;
00312         if(-f < _1in0.mCenter.x)        return FALSE;
00313 
00314         f = fabsf(mtx.m[0][1] * mExtents.x) + fabsf(mtx.m[1][1] * mExtents.y) + fabsf(mtx.m[2][1] * mExtents.z) - box.mExtents.y;
00315         if(f > _1in0.mCenter.y)         return FALSE;
00316         if(-f < _1in0.mCenter.y)        return FALSE;
00317 
00318         f = fabsf(mtx.m[0][2] * mExtents.x) + fabsf(mtx.m[1][2] * mExtents.y) + fabsf(mtx.m[2][2] * mExtents.z) - box.mExtents.z;
00319         if(f > _1in0.mCenter.z)         return FALSE;
00320         if(-f < _1in0.mCenter.z)        return FALSE;
00321 
00322         return TRUE;
00323 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:54