IceOBB.cpp
Go to the documentation of this file.
1 
8 
11 
17 
20 // Precompiled Header
21 #include "Stdafx.h"
22 
23 using namespace IceMaths;
24 
26 
31 bool OBB::ContainsPoint(const Point& p) const
33 {
34  // Point in OBB test using lazy evaluation and early exits
35 
36  // Translate to box space
37  Point RelPoint = p - mCenter;
38 
39  // Point * mRot maps from box space to world space
40  // mRot * Point maps from world space to box space (what we need here)
41 
42  float f = mRot.m[0][0] * RelPoint.x + mRot.m[0][1] * RelPoint.y + mRot.m[0][2] * RelPoint.z;
43  if(f >= mExtents.x || f <= -mExtents.x) return false;
44 
45  f = mRot.m[1][0] * RelPoint.x + mRot.m[1][1] * RelPoint.y + mRot.m[1][2] * RelPoint.z;
46  if(f >= mExtents.y || f <= -mExtents.y) return false;
47 
48  f = mRot.m[2][0] * RelPoint.x + mRot.m[2][1] * RelPoint.y + mRot.m[2][2] * RelPoint.z;
49  if(f >= mExtents.z || f <= -mExtents.z) return false;
50  return true;
51 }
52 
54 
59 void OBB::Create(const AABB& aabb, const Matrix4x4& mat)
61 {
62  // Note: must be coherent with Rotate()
63 
64  aabb.GetCenter(mCenter);
65  aabb.GetExtents(mExtents);
66  // Here we have the same as OBB::Rotate(mat) where the obb is (mCenter, mExtents, Identity).
67 
68  // So following what's done in Rotate:
69  // - x-form the center
70  mCenter *= mat;
71  // - combine rotation with identity, i.e. just use given matrix
72  mRot = mat;
73 }
74 
76 
81 bool OBB::ComputePlanes(Plane* planes) const
83 {
84  // Checkings
85  if(!planes) return false;
86 
87  Point Axis0 = mRot[0];
88  Point Axis1 = mRot[1];
89  Point Axis2 = mRot[2];
90 
91  // Writes normals
92  planes[0].n = Axis0;
93  planes[1].n = -Axis0;
94  planes[2].n = Axis1;
95  planes[3].n = -Axis1;
96  planes[4].n = Axis2;
97  planes[5].n = -Axis2;
98 
99  // Compute a point on each plane
100  Point p0 = mCenter + Axis0 * mExtents.x;
101  Point p1 = mCenter - Axis0 * mExtents.x;
102  Point p2 = mCenter + Axis1 * mExtents.y;
103  Point p3 = mCenter - Axis1 * mExtents.y;
104  Point p4 = mCenter + Axis2 * mExtents.z;
105  Point p5 = mCenter - Axis2 * mExtents.z;
106 
107  // Compute d
108  planes[0].d = -(planes[0].n|p0);
109  planes[1].d = -(planes[1].n|p1);
110  planes[2].d = -(planes[2].n|p2);
111  planes[3].d = -(planes[3].n|p3);
112  planes[4].d = -(planes[4].n|p4);
113  planes[5].d = -(planes[5].n|p5);
114 
115  return true;
116 }
117 
119 
124 bool OBB::ComputePoints(Point* pts) const
126 {
127  // Checkings
128  if(!pts) return false;
129 
130  Point Axis0 = mRot[0];
131  Point Axis1 = mRot[1];
132  Point Axis2 = mRot[2];
133 
134  Axis0 *= mExtents.x;
135  Axis1 *= mExtents.y;
136  Axis2 *= mExtents.z;
137 
138  // 7+------+6 0 = ---
139  // /| /| 1 = +--
140  // / | / | 2 = ++-
141  // / 4+---/--+5 3 = -+-
142  // 3+------+2 / y z 4 = --+
143  // | / | / | / 5 = +-+
144  // |/ |/ |/ 6 = +++
145  // 0+------+1 *---x 7 = -++
146 
147  pts[0] = mCenter - Axis0 - Axis1 - Axis2;
148  pts[1] = mCenter + Axis0 - Axis1 - Axis2;
149  pts[2] = mCenter + Axis0 + Axis1 - Axis2;
150  pts[3] = mCenter - Axis0 + Axis1 - Axis2;
151  pts[4] = mCenter - Axis0 - Axis1 + Axis2;
152  pts[5] = mCenter + Axis0 - Axis1 + Axis2;
153  pts[6] = mCenter + Axis0 + Axis1 + Axis2;
154  pts[7] = mCenter - Axis0 + Axis1 + Axis2;
155 
156  return true;
157 }
158 
160 
165 bool OBB::ComputeVertexNormals(Point* pts) const
167 {
168  static float VertexNormals[] =
169  {
178  };
179 
180  if(!pts) return false;
181 
182  const Point* VN = (const Point*)VertexNormals;
183  for(udword i=0;i<8;i++)
184  {
185  pts[i] = VN[i] * mRot;
186  }
187 
188  return true;
189 }
190 
192 
196 const udword* OBB::GetEdges() const
198 {
199  static udword Indices[] = {
200  0, 1, 1, 2, 2, 3, 3, 0,
201  7, 6, 6, 5, 5, 4, 4, 7,
202  1, 5, 6, 2,
203  3, 7, 4, 0
204  };
205  return Indices;
206 }
207 
209 
213 const Point* OBB::GetLocalEdgeNormals() const
215 {
216  static float EdgeNormals[] =
217  {
218  0, -INVSQRT2, -INVSQRT2, // 0-1
219  INVSQRT2, 0, -INVSQRT2, // 1-2
220  0, INVSQRT2, -INVSQRT2, // 2-3
221  -INVSQRT2, 0, -INVSQRT2, // 3-0
222 
223  0, INVSQRT2, INVSQRT2, // 7-6
224  INVSQRT2, 0, INVSQRT2, // 6-5
225  0, -INVSQRT2, INVSQRT2, // 5-4
226  -INVSQRT2, 0, INVSQRT2, // 4-7
227 
228  INVSQRT2, -INVSQRT2, 0, // 1-5
229  INVSQRT2, INVSQRT2, 0, // 6-2
230  -INVSQRT2, INVSQRT2, 0, // 3-7
231  -INVSQRT2, -INVSQRT2, 0 // 4-0
232  };
233  return (const Point*)EdgeNormals;
234 }
235 
237 
242 void OBB::ComputeWorldEdgeNormal(udword edge_index, Point& world_normal) const
244 {
245  ASSERT(edge_index<12);
246  world_normal = GetLocalEdgeNormals()[edge_index] * mRot;
247 }
248 
250 
254 void OBB::ComputeLSS(LSS& lss) const
256 {
257  Point Axis0 = mRot[0];
258  Point Axis1 = mRot[1];
259  Point Axis2 = mRot[2];
260 
261  switch(mExtents.LargestAxis())
262  {
263  case 0:
264  lss.mRadius = (mExtents.y + mExtents.z)*0.5f;
265  lss.mP0 = mCenter + Axis0 * (mExtents.x - lss.mRadius);
266  lss.mP1 = mCenter - Axis0 * (mExtents.x - lss.mRadius);
267  break;
268  case 1:
269  lss.mRadius = (mExtents.x + mExtents.z)*0.5f;
270  lss.mP0 = mCenter + Axis1 * (mExtents.y - lss.mRadius);
271  lss.mP1 = mCenter - Axis1 * (mExtents.y - lss.mRadius);
272  break;
273  case 2:
274  lss.mRadius = (mExtents.x + mExtents.y)*0.5f;
275  lss.mP0 = mCenter + Axis2 * (mExtents.z - lss.mRadius);
276  lss.mP1 = mCenter - Axis2 * (mExtents.z - lss.mRadius);
277  break;
278  }
279 }
280 
282 
287 BOOL OBB::IsInside(const OBB& box) const
289 {
290  // Make a 4x4 from the box & inverse it
291  Matrix4x4 M0Inv;
292  {
293  Matrix4x4 M0 = box.mRot;
294  M0.SetTrans(box.mCenter);
295  InvertPRMatrix(M0Inv, M0);
296  }
297 
298  // With our inversed 4x4, create box1 in space of box0
299  OBB _1in0;
300  Rotate(M0Inv, _1in0);
301 
302  // This should cancel out box0's rotation, i.e. it's now an AABB.
303  // => Center(0,0,0), Rot(identity)
304 
305  // The two boxes are in the same space so now we can compare them.
306 
307  // Create the AABB of (box1 in space of box0)
308  const Matrix3x3& mtx = _1in0.mRot;
309 
310  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;
311  if(f > _1in0.mCenter.x) return FALSE;
312  if(-f < _1in0.mCenter.x) return FALSE;
313 
314  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;
315  if(f > _1in0.mCenter.y) return FALSE;
316  if(-f < _1in0.mCenter.y) return FALSE;
317 
318  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;
319  if(f > _1in0.mCenter.z) return FALSE;
320  if(-f < _1in0.mCenter.z) return FALSE;
321 
322  return TRUE;
323 }
OBB::GetLocalEdgeNormals
const Point * GetLocalEdgeNormals() const
Definition: IceOBB.cpp:214
i
png_uint_32 i
Definition: png.h:2732
OBB::ContainsPoint
bool ContainsPoint(const Point &p) const
Definition: IceOBB.cpp:32
Matrix4x4
Definition: IceMatrix4x4.h:21
OBB::ComputeVertexNormals
bool ComputeVertexNormals(Point *pts) const
Definition: IceOBB.cpp:166
ASSERT
#define ASSERT(exp)
Definition: OPC_IceHook.h:24
OBB::IsInside
BOOL IsInside(const OBB &box) const
Definition: IceOBB.cpp:288
LSS
Definition: IceLSS.h:15
Segment::mP0
Point mP0
Start of segment.
Definition: IceSegment.h:51
Segment::mP1
Point mP1
End of segment.
Definition: IceSegment.h:52
OBB::ComputeLSS
void ComputeLSS(LSS &lss) const
Definition: IceOBB.cpp:255
box
Definition: jquant2.c:258
udword
unsigned int udword
sizeof(udword) must be 4
Definition: IceTypes.h:65
OBB::GetEdges
const udword * GetEdges() const
Definition: IceOBB.cpp:197
BOOL
int BOOL
Another boolean type.
Definition: IceTypes.h:102
Matrix4x4::SetTrans
inline_ void SetTrans(const Point &p)
Sets the translation part of the matrix, from a Point.
Definition: IceMatrix4x4.h:99
swingTest.f
f
Definition: swingTest.py:6
AABB::GetCenter
inline_ void GetCenter(Point &center) const
Get box center.
Definition: IceAABB.h:353
OBB::Create
void Create(const AABB &aabb, const Matrix4x4 &mat)
Definition: IceOBB.cpp:60
InvertPRMatrix
ICEMATHS_API void InvertPRMatrix(Matrix4x4 &dest, const Matrix4x4 &src)
OBB::mRot
Matrix3x3 mRot
O for Oriented.
Definition: IceOBB.h:171
OBB::mCenter
Point mCenter
B for Box.
Definition: IceOBB.h:169
OBB::ComputePlanes
bool ComputePlanes(Plane *planes) const
Definition: IceOBB.cpp:82
Plane::n
Point n
The normal to the plane.
Definition: IcePlane.h:53
Stdafx.h
Plane::d
float d
The distance from the origin.
Definition: IcePlane.h:54
INVSQRT3
#define INVSQRT3
1 / sqrt(3)
Definition: IceTypes.h:55
Matrix3x3::m
float m[3][3]
Definition: IceMatrix3x3.h:492
AABB::GetExtents
inline_ void GetExtents(Point &extents) const
Get box extents.
Definition: IceAABB.h:355
LSS::mRadius
float mRadius
Sphere radius.
Definition: IceLSS.h:72
TRUE
#define TRUE
Definition: OPC_IceHook.h:13
OBB
Definition: IceOBB.h:18
OBB::ComputeWorldEdgeNormal
void ComputeWorldEdgeNormal(udword edge_index, Point &world_normal) const
Definition: IceOBB.cpp:243
FALSE
#define FALSE
Definition: OPC_IceHook.h:9
Plane
Definition: IcePlane.h:17
Matrix3x3
Definition: IceMatrix3x3.h:20
OBB::ComputePoints
bool ComputePoints(Point *pts) const
Definition: IceOBB.cpp:125
INVSQRT2
#define INVSQRT2
1 / sqrt(2)
Definition: IceTypes.h:52
IceMaths
Definition: ColdetModel.h:24
AABB
Definition: IceAABB.h:267


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:03