← Back to branch summary

~ubuntu-branches/ubuntu/lucid/blender/lucid

« back to all changes in this revision

Viewing changes to extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h

  • Committer: Bazaar Package Importer
  • Author(s): Chris Coulson
  • Date: 2009-08-06 22:32:19 UTC
  • mfrom: (1.2.10 upstream)
  • Revision ID: james.westby@ubuntu.com-20090806223219-8z4eej1u8levu4pz
Tags: 2.49a+dfsg-0ubuntu1
https://launchpad.net/bugs/382153
* Merge from debian unstable, remaining changes:
  - debian/control: Build-depend on python-2.6 rather than python-2.5.
  - debian/misc/*.desktop: Add Spanish translation to .desktop 
    files.
  - debian/pyversions: 2.6.
  - debian/rules: Clean *.o of source/blender/python/api2_2x/
* New upstream release (LP: #382153).
* Refreshed patches:
  - 01_sanitize_sys.patch
  - 02_tmp_in_HOME
  - 10_use_systemwide_ftgl
  - 70_portability_platform_detection
* Removed patches merged upstream:
  - 30_fix_python_syntax_warning
  - 90_ubuntu_ffmpeg_52_changes

Show diffs side-by-side

added added

removed removed

Lines of Context:
extern/bullet2/src/BulletCollision/Gimpact/gim_box_collision.h
1
 
#ifndef GIM_BOX_COLLISION_H_INCLUDED
2
 
#define GIM_BOX_COLLISION_H_INCLUDED
3
 
 
4
 
/*! \file gim_box_collision.h
5
 
\author Francisco Len N�jera
6
 
*/
7
 
/*
8
 
-----------------------------------------------------------------------------
9
 
This source file is part of GIMPACT Library.
10
 
 
11
 
For the latest info, see http://gimpact.sourceforge.net/
12
 
 
13
 
Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
14
 
email: projectileman@yahoo.com
15
 
 
16
 
 This library is free software; you can redistribute it and/or
17
 
 modify it under the terms of EITHER:
18
 
   (1) The GNU Lesser General Public License as published by the Free
19
 
       Software Foundation; either version 2.1 of the License, or (at
20
 
       your option) any later version. The text of the GNU Lesser
21
 
       General Public License is included with this library in the
22
 
       file GIMPACT-LICENSE-LGPL.TXT.
23
 
   (2) The BSD-style license that is included with this library in
24
 
       the file GIMPACT-LICENSE-BSD.TXT.
25
 
   (3) The zlib/libpng license that is included with this library in
26
 
       the file GIMPACT-LICENSE-ZLIB.TXT.
27
 
 
28
 
 This library is distributed in the hope that it will be useful,
29
 
 but WITHOUT ANY WARRANTY; without even the implied warranty of
30
 
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
31
 
 GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
32
 
 
33
 
-----------------------------------------------------------------------------
34
 
*/
35
 
#include "gim_basic_geometry_operations.h"
36
 
#include "LinearMath/btTransform.h"
37
 
 
38
 
/*! \defgroup BOUND_AABB_OPERATIONS
39
 
*/
40
 
//! @{
41
 
 
42
 
 
43
 
//SIMD_FORCE_INLINE bool test_cross_edge_box(
44
 
//      const btVector3 & edge,
45
 
//      const btVector3 & absolute_edge,
46
 
//      const btVector3 & pointa,
47
 
//      const btVector3 & pointb, const btVector3 & extend,
48
 
//      int dir_index0,
49
 
//      int dir_index1
50
 
//      int component_index0,
51
 
//      int component_index1)
52
 
//{
53
 
//      // dir coords are -z and y
54
 
//
55
 
//      const btScalar dir0 = -edge[dir_index0];
56
 
//      const btScalar dir1 = edge[dir_index1];
57
 
//      btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1;
58
 
//      btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1;
59
 
//      //find minmax
60
 
//      if(pmin>pmax)
61
 
//      {
62
 
//              GIM_SWAP_NUMBERS(pmin,pmax);
63
 
//      }
64
 
//      //find extends
65
 
//      const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] +
66
 
//                                      extend[component_index1] * absolute_edge[dir_index1];
67
 
//
68
 
//      if(pmin>rad || -rad>pmax) return false;
69
 
//      return true;
70
 
//}
71
 
//
72
 
//SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis(
73
 
//      const btVector3 & edge,
74
 
//      const btVector3 & absolute_edge,
75
 
//      const btVector3 & pointa,
76
 
//      const btVector3 & pointb, btVector3 & extend)
77
 
//{
78
 
//
79
 
//      return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2);
80
 
//}
81
 
//
82
 
//
83
 
//SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis(
84
 
//      const btVector3 & edge,
85
 
//      const btVector3 & absolute_edge,
86
 
//      const btVector3 & pointa,
87
 
//      const btVector3 & pointb, btVector3 & extend)
88
 
//{
89
 
//
90
 
//      return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0);
91
 
//}
92
 
//
93
 
//SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis(
94
 
//      const btVector3 & edge,
95
 
//      const btVector3 & absolute_edge,
96
 
//      const btVector3 & pointa,
97
 
//      const btVector3 & pointb, btVector3 & extend)
98
 
//{
99
 
//
100
 
//      return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1);
101
 
//}
102
 
 
103
 
#define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
104
 
{\
105
 
        const btScalar dir0 = -edge[i_dir_0];\
106
 
        const btScalar dir1 = edge[i_dir_1];\
107
 
        btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\
108
 
        btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\
109
 
        if(pmin>pmax)\
110
 
        {\
111
 
                GIM_SWAP_NUMBERS(pmin,pmax); \
112
 
        }\
113
 
        const btScalar abs_dir0 = absolute_edge[i_dir_0];\
114
 
        const btScalar abs_dir1 = absolute_edge[i_dir_1];\
115
 
        const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\
116
 
        if(pmin>rad || -rad>pmax) return false;\
117
 
}\
118
 
 
119
 
 
120
 
#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
121
 
{\
122
 
        TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\
123
 
}\
124
 
 
125
 
#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
126
 
{\
127
 
        TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\
128
 
}\
129
 
 
130
 
#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
131
 
{\
132
 
        TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\
133
 
}\
134
 
 
135
 
 
136
 
 
137
 
//!  Class for transforming a model1 to the space of model0
138
 
class GIM_BOX_BOX_TRANSFORM_CACHE
139
 
{
140
 
public:
141
 
    btVector3  m_T1to0;//!< Transforms translation of model1 to model 0
142
 
        btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal  to R0' * R1
143
 
        btMatrix3x3 m_AR;//!< Absolute value of m_R1to0
144
 
 
145
 
        SIMD_FORCE_INLINE void calc_absolute_matrix()
146
 
        {
147
 
                static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
148
 
                m_AR[0] = vepsi + m_R1to0[0].absolute();
149
 
                m_AR[1] = vepsi + m_R1to0[1].absolute();
150
 
                m_AR[2] = vepsi + m_R1to0[2].absolute();
151
 
        }
152
 
 
153
 
        GIM_BOX_BOX_TRANSFORM_CACHE()
154
 
        {
155
 
        }
156
 
 
157
 
 
158
 
        GIM_BOX_BOX_TRANSFORM_CACHE(mat4f  trans1_to_0)
159
 
        {
160
 
                COPY_MATRIX_3X3(m_R1to0,trans1_to_0)
161
 
        MAT_GET_TRANSLATION(trans1_to_0,m_T1to0)
162
 
                calc_absolute_matrix();
163
 
        }
164
 
 
165
 
        //! Calc the transformation relative  1 to 0. Inverts matrics by transposing
166
 
        SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1)
167
 
        {
168
 
 
169
 
                m_R1to0 = trans0.getBasis().transpose();
170
 
                m_T1to0 = m_R1to0 * (-trans0.getOrigin());
171
 
 
172
 
                m_T1to0 += m_R1to0*trans1.getOrigin();
173
 
                m_R1to0 *= trans1.getBasis();
174
 
 
175
 
                calc_absolute_matrix();
176
 
        }
177
 
 
178
 
        //! Calcs the full invertion of the matrices. Useful for scaling matrices
179
 
        SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1)
180
 
        {
181
 
                m_R1to0 = trans0.getBasis().inverse();
182
 
                m_T1to0 = m_R1to0 * (-trans0.getOrigin());
183
 
 
184
 
                m_T1to0 += m_R1to0*trans1.getOrigin();
185
 
                m_R1to0 *= trans1.getBasis();
186
 
 
187
 
                calc_absolute_matrix();
188
 
        }
189
 
 
190
 
        SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point)
191
 
        {
192
 
                return btVector3(m_R1to0[0].dot(point) + m_T1to0.x(),
193
 
                        m_R1to0[1].dot(point) + m_T1to0.y(),
194
 
                        m_R1to0[2].dot(point) + m_T1to0.z());
195
 
        }
196
 
};
197
 
 
198
 
 
199
 
#define BOX_PLANE_EPSILON 0.000001f
200
 
 
201
 
//! Axis aligned box
202
 
class GIM_AABB
203
 
{
204
 
public:
205
 
        btVector3 m_min;
206
 
        btVector3 m_max;
207
 
 
208
 
        GIM_AABB()
209
 
        {}
210
 
 
211
 
 
212
 
        GIM_AABB(const btVector3 & V1,
213
 
                         const btVector3 & V2,
214
 
                         const btVector3 & V3)
215
 
        {
216
 
                m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
217
 
                m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
218
 
                m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
219
 
 
220
 
                m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
221
 
                m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
222
 
                m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
223
 
        }
224
 
 
225
 
        GIM_AABB(const btVector3 & V1,
226
 
                         const btVector3 & V2,
227
 
                         const btVector3 & V3,
228
 
                         GREAL margin)
229
 
        {
230
 
                m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
231
 
                m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
232
 
                m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
233
 
 
234
 
                m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
235
 
                m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
236
 
                m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
237
 
 
238
 
                m_min[0] -= margin;
239
 
                m_min[1] -= margin;
240
 
                m_min[2] -= margin;
241
 
                m_max[0] += margin;
242
 
                m_max[1] += margin;
243
 
                m_max[2] += margin;
244
 
        }
245
 
 
246
 
        GIM_AABB(const GIM_AABB &other):
247
 
                m_min(other.m_min),m_max(other.m_max)
248
 
        {
249
 
        }
250
 
 
251
 
        GIM_AABB(const GIM_AABB &other,btScalar margin ):
252
 
                m_min(other.m_min),m_max(other.m_max)
253
 
        {
254
 
                m_min[0] -= margin;
255
 
                m_min[1] -= margin;
256
 
                m_min[2] -= margin;
257
 
                m_max[0] += margin;
258
 
                m_max[1] += margin;
259
 
                m_max[2] += margin;
260
 
        }
261
 
 
262
 
        SIMD_FORCE_INLINE void invalidate()
263
 
        {
264
 
                m_min[0] = G_REAL_INFINITY;
265
 
                m_min[1] = G_REAL_INFINITY;
266
 
                m_min[2] = G_REAL_INFINITY;
267
 
                m_max[0] = -G_REAL_INFINITY;
268
 
                m_max[1] = -G_REAL_INFINITY;
269
 
                m_max[2] = -G_REAL_INFINITY;
270
 
        }
271
 
 
272
 
        SIMD_FORCE_INLINE void increment_margin(btScalar margin)
273
 
        {
274
 
                m_min[0] -= margin;
275
 
                m_min[1] -= margin;
276
 
                m_min[2] -= margin;
277
 
                m_max[0] += margin;
278
 
                m_max[1] += margin;
279
 
                m_max[2] += margin;
280
 
        }
281
 
 
282
 
        SIMD_FORCE_INLINE void copy_with_margin(const GIM_AABB &other, btScalar margin)
283
 
        {
284
 
                m_min[0] = other.m_min[0] - margin;
285
 
                m_min[1] = other.m_min[1] - margin;
286
 
                m_min[2] = other.m_min[2] - margin;
287
 
 
288
 
                m_max[0] = other.m_max[0] + margin;
289
 
                m_max[1] = other.m_max[1] + margin;
290
 
                m_max[2] = other.m_max[2] + margin;
291
 
        }
292
 
 
293
 
        template<typename CLASS_POINT>
294
 
        SIMD_FORCE_INLINE void calc_from_triangle(
295
 
                                                        const CLASS_POINT & V1,
296
 
                                                        const CLASS_POINT & V2,
297
 
                                                        const CLASS_POINT & V3)
298
 
        {
299
 
                m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
300
 
                m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
301
 
                m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
302
 
 
303
 
                m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
304
 
                m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
305
 
                m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
306
 
        }
307
 
 
308
 
        template<typename CLASS_POINT>
309
 
        SIMD_FORCE_INLINE void calc_from_triangle_margin(
310
 
                                                        const CLASS_POINT & V1,
311
 
                                                        const CLASS_POINT & V2,
312
 
                                                        const CLASS_POINT & V3, btScalar margin)
313
 
        {
314
 
                m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
315
 
                m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
316
 
                m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
317
 
 
318
 
                m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
319
 
                m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
320
 
                m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
321
 
 
322
 
                m_min[0] -= margin;
323
 
                m_min[1] -= margin;
324
 
                m_min[2] -= margin;
325
 
                m_max[0] += margin;
326
 
                m_max[1] += margin;
327
 
                m_max[2] += margin;
328
 
        }
329
 
 
330
 
        //! Apply a transform to an AABB
331
 
        SIMD_FORCE_INLINE void appy_transform(const btTransform & trans)
332
 
        {
333
 
                btVector3 center = (m_max+m_min)*0.5f;
334
 
                btVector3 extends = m_max - center;
335
 
                // Compute new center
336
 
                center = trans(center);
337
 
 
338
 
                btVector3 textends(extends.dot(trans.getBasis().getRow(0).absolute()),
339
 
                                 extends.dot(trans.getBasis().getRow(1).absolute()),
340
 
                                 extends.dot(trans.getBasis().getRow(2).absolute()));
341
 
 
342
 
                m_min = center - textends;
343
 
                m_max = center + textends;
344
 
        }
345
 
 
346
 
        //! Merges a Box
347
 
        SIMD_FORCE_INLINE void merge(const GIM_AABB & box)
348
 
        {
349
 
                m_min[0] = GIM_MIN(m_min[0],box.m_min[0]);
350
 
                m_min[1] = GIM_MIN(m_min[1],box.m_min[1]);
351
 
                m_min[2] = GIM_MIN(m_min[2],box.m_min[2]);
352
 
 
353
 
                m_max[0] = GIM_MAX(m_max[0],box.m_max[0]);
354
 
                m_max[1] = GIM_MAX(m_max[1],box.m_max[1]);
355
 
                m_max[2] = GIM_MAX(m_max[2],box.m_max[2]);
356
 
        }
357
 
 
358
 
        //! Merges a point
359
 
        template<typename CLASS_POINT>
360
 
        SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point)
361
 
        {
362
 
                m_min[0] = GIM_MIN(m_min[0],point[0]);
363
 
                m_min[1] = GIM_MIN(m_min[1],point[1]);
364
 
                m_min[2] = GIM_MIN(m_min[2],point[2]);
365
 
 
366
 
                m_max[0] = GIM_MAX(m_max[0],point[0]);
367
 
                m_max[1] = GIM_MAX(m_max[1],point[1]);
368
 
                m_max[2] = GIM_MAX(m_max[2],point[2]);
369
 
        }
370
 
 
371
 
        //! Gets the extend and center
372
 
        SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend)  const
373
 
        {
374
 
                center = (m_max+m_min)*0.5f;
375
 
                extend = m_max - center;
376
 
        }
377
 
 
378
 
        //! Finds the intersecting box between this box and the other.
379
 
        SIMD_FORCE_INLINE void find_intersection(const GIM_AABB & other, GIM_AABB & intersection)  const
380
 
        {
381
 
                intersection.m_min[0] = GIM_MAX(other.m_min[0],m_min[0]);
382
 
                intersection.m_min[1] = GIM_MAX(other.m_min[1],m_min[1]);
383
 
                intersection.m_min[2] = GIM_MAX(other.m_min[2],m_min[2]);
384
 
 
385
 
                intersection.m_max[0] = GIM_MIN(other.m_max[0],m_max[0]);
386
 
                intersection.m_max[1] = GIM_MIN(other.m_max[1],m_max[1]);
387
 
                intersection.m_max[2] = GIM_MIN(other.m_max[2],m_max[2]);
388
 
        }
389
 
 
390
 
 
391
 
        SIMD_FORCE_INLINE bool has_collision(const GIM_AABB & other) const
392
 
        {
393
 
                if(m_min[0] > other.m_max[0] ||
394
 
                   m_max[0] < other.m_min[0] ||
395
 
                   m_min[1] > other.m_max[1] ||
396
 
                   m_max[1] < other.m_min[1] ||
397
 
                   m_min[2] > other.m_max[2] ||
398
 
                   m_max[2] < other.m_min[2])
399
 
                {
400
 
                        return false;
401
 
                }
402
 
                return true;
403
 
        }
404
 
 
405
 
        /*! \brief Finds the Ray intersection parameter.
406
 
        \param aabb Aligned box
407
 
        \param vorigin A vec3f with the origin of the ray
408
 
        \param vdir A vec3f with the direction of the ray
409
 
        */
410
 
        SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir)
411
 
        {
412
 
                btVector3 extents,center;
413
 
                this->get_center_extend(center,extents);;
414
 
 
415
 
                btScalar Dx = vorigin[0] - center[0];
416
 
                if(GIM_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f)     return false;
417
 
                btScalar Dy = vorigin[1] - center[1];
418
 
                if(GIM_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f)     return false;
419
 
                btScalar Dz = vorigin[2] - center[2];
420
 
                if(GIM_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f)     return false;
421
 
 
422
 
 
423
 
                btScalar f = vdir[1] * Dz - vdir[2] * Dy;
424
 
                if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false;
425
 
                f = vdir[2] * Dx - vdir[0] * Dz;
426
 
                if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false;
427
 
                f = vdir[0] * Dy - vdir[1] * Dx;
428
 
                if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false;
429
 
                return true;
430
 
        }
431
 
 
432
 
 
433
 
        SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const
434
 
        {
435
 
                btVector3 center = (m_max+m_min)*0.5f;
436
 
                btVector3 extend = m_max-center;
437
 
 
438
 
                btScalar _fOrigin =  direction.dot(center);
439
 
                btScalar _fMaximumExtent = extend.dot(direction.absolute());
440
 
                vmin = _fOrigin - _fMaximumExtent;
441
 
                vmax = _fOrigin + _fMaximumExtent;
442
 
        }
443
 
 
444
 
        SIMD_FORCE_INLINE ePLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const
445
 
        {
446
 
                btScalar _fmin,_fmax;
447
 
                this->projection_interval(plane,_fmin,_fmax);
448
 
 
449
 
                if(plane[3] > _fmax + BOX_PLANE_EPSILON)
450
 
                {
451
 
                        return G_BACK_PLANE; // 0
452
 
                }
453
 
 
454
 
                if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
455
 
                {
456
 
                        return G_COLLIDE_PLANE; //1
457
 
                }
458
 
                return G_FRONT_PLANE;//2
459
 
        }
460
 
 
461
 
        SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB & box, btTransform & trans1_to_0)
462
 
        {
463
 
                GIM_AABB tbox = box;
464
 
                tbox.appy_transform(trans1_to_0);
465
 
                return has_collision(tbox);
466
 
        }
467
 
 
468
 
        //! transcache is the transformation cache from box to this AABB
469
 
        SIMD_FORCE_INLINE bool overlapping_trans_cache(
470
 
                const GIM_AABB & box,const GIM_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest)
471
 
        {
472
 
 
473
 
                //Taken from OPCODE
474
 
                btVector3 ea,eb;//extends
475
 
                btVector3 ca,cb;//extends
476
 
                get_center_extend(ca,ea);
477
 
                box.get_center_extend(cb,eb);
478
 
 
479
 
 
480
 
                btVector3 T;
481
 
                btScalar t,t2;
482
 
                int i;
483
 
 
484
 
                // Class I : A's basis vectors
485
 
                for(i=0;i<3;i++)
486
 
                {
487
 
                        T[i] =  transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
488
 
                        t = transcache.m_AR[i].dot(eb) + ea[i];
489
 
                        if(GIM_GREATER(T[i], t))        return false;
490
 
                }
491
 
                // Class II : B's basis vectors
492
 
                for(i=0;i<3;i++)
493
 
                {
494
 
                        t = MAT_DOT_COL(transcache.m_R1to0,T,i);
495
 
                        t2 = MAT_DOT_COL(transcache.m_AR,ea,i) + eb[i];
496
 
                        if(GIM_GREATER(t,t2))   return false;
497
 
                }
498
 
                // Class III : 9 cross products
499
 
                if(fulltest)
500
 
                {
501
 
                        int j,m,n,o,p,q,r;
502
 
                        for(i=0;i<3;i++)
503
 
                        {
504
 
                                m = (i+1)%3;
505
 
                                n = (i+2)%3;
506
 
                                o = i==0?1:0;
507
 
                                p = i==2?1:2;
508
 
                                for(j=0;j<3;j++)
509
 
                                {
510
 
                                        q = j==2?1:2;
511
 
                                        r = j==0?1:0;
512
 
                                        t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j];
513
 
                                        t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] +
514
 
                                                eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r];
515
 
                                        if(GIM_GREATER(t,t2))   return false;
516
 
                                }
517
 
                        }
518
 
                }
519
 
                return true;
520
 
        }
521
 
 
522
 
        //! Simple test for planes.
523
 
        SIMD_FORCE_INLINE bool collide_plane(
524
 
                const btVector4 & plane)
525
 
        {
526
 
                ePLANE_INTERSECTION_TYPE classify = plane_classify(plane);
527
 
                return (classify == G_COLLIDE_PLANE);
528
 
        }
529
 
 
530
 
        //! test for a triangle, with edges
531
 
        SIMD_FORCE_INLINE bool collide_triangle_exact(
532
 
                const btVector3 & p1,
533
 
                const btVector3 & p2,
534
 
                const btVector3 & p3,
535
 
                const btVector4 & triangle_plane)
536
 
        {
537
 
                if(!collide_plane(triangle_plane)) return false;
538
 
 
539
 
                btVector3 center,extends;
540
 
                this->get_center_extend(center,extends);
541
 
 
542
 
                const btVector3 v1(p1 - center);
543
 
                const btVector3 v2(p2 - center);
544
 
                const btVector3 v3(p3 - center);
545
 
 
546
 
                //First axis
547
 
                btVector3 diff(v2 - v1);
548
 
                btVector3 abs_diff = diff.absolute();
549
 
                //Test With X axis
550
 
                TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
551
 
                //Test With Y axis
552
 
                TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
553
 
                //Test With Z axis
554
 
                TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends);
555
 
 
556
 
 
557
 
                diff = v3 - v2;
558
 
                abs_diff = diff.absolute();
559
 
                //Test With X axis
560
 
                TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
561
 
                //Test With Y axis
562
 
                TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
563
 
                //Test With Z axis
564
 
                TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends);
565
 
 
566
 
                diff = v1 - v3;
567
 
                abs_diff = diff.absolute();
568
 
                //Test With X axis
569
 
                TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
570
 
                //Test With Y axis
571
 
                TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
572
 
                //Test With Z axis
573
 
                TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends);
574
 
 
575
 
                return true;
576
 
        }
577
 
};
578
 
 
579
 
 
580
 
//! Compairison of transformation objects
581
 
SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
582
 
{
583
 
        if(!(t1.getOrigin() == t2.getOrigin()) ) return false;
584
 
 
585
 
        if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false;
586
 
        if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false;
587
 
        if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
588
 
        return true;
589
 
}
590
 
 
591
 
 
592
 
//! @}
593
 
 
594
 
#endif // GIM_BOX_COLLISION_H_INCLUDED
 
1
#ifndef GIM_BOX_COLLISION_H_INCLUDED
 
2
#define GIM_BOX_COLLISION_H_INCLUDED
 
3
 
 
4
/*! \file gim_box_collision.h
 
5
\author Francisco Len N�jera
 
6
*/
 
7
/*
 
8
-----------------------------------------------------------------------------
 
9
This source file is part of GIMPACT Library.
 
10
 
 
11
For the latest info, see http://gimpact.sourceforge.net/
 
12
 
 
13
Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
 
14
email: projectileman@yahoo.com
 
15
 
 
16
 This library is free software; you can redistribute it and/or
 
17
 modify it under the terms of EITHER:
 
18
   (1) The GNU Lesser General Public License as published by the Free
 
19
       Software Foundation; either version 2.1 of the License, or (at
 
20
       your option) any later version. The text of the GNU Lesser
 
21
       General Public License is included with this library in the
 
22
       file GIMPACT-LICENSE-LGPL.TXT.
 
23
   (2) The BSD-style license that is included with this library in
 
24
       the file GIMPACT-LICENSE-BSD.TXT.
 
25
   (3) The zlib/libpng license that is included with this library in
 
26
       the file GIMPACT-LICENSE-ZLIB.TXT.
 
27
 
 
28
 This library is distributed in the hope that it will be useful,
 
29
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 
30
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
 
31
 GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
 
32
 
 
33
-----------------------------------------------------------------------------
 
34
*/
 
35
#include "gim_basic_geometry_operations.h"
 
36
#include "LinearMath/btTransform.h"
 
37
 
 
38
 
 
39
 
 
40
//SIMD_FORCE_INLINE bool test_cross_edge_box(
 
41
//      const btVector3 & edge,
 
42
//      const btVector3 & absolute_edge,
 
43
//      const btVector3 & pointa,
 
44
//      const btVector3 & pointb, const btVector3 & extend,
 
45
//      int dir_index0,
 
46
//      int dir_index1
 
47
//      int component_index0,
 
48
//      int component_index1)
 
49
//{
 
50
//      // dir coords are -z and y
 
51
//
 
52
//      const btScalar dir0 = -edge[dir_index0];
 
53
//      const btScalar dir1 = edge[dir_index1];
 
54
//      btScalar pmin = pointa[component_index0]*dir0 + pointa[component_index1]*dir1;
 
55
//      btScalar pmax = pointb[component_index0]*dir0 + pointb[component_index1]*dir1;
 
56
//      //find minmax
 
57
//      if(pmin>pmax)
 
58
//      {
 
59
//              GIM_SWAP_NUMBERS(pmin,pmax);
 
60
//      }
 
61
//      //find extends
 
62
//      const btScalar rad = extend[component_index0] * absolute_edge[dir_index0] +
 
63
//                                      extend[component_index1] * absolute_edge[dir_index1];
 
64
//
 
65
//      if(pmin>rad || -rad>pmax) return false;
 
66
//      return true;
 
67
//}
 
68
//
 
69
//SIMD_FORCE_INLINE bool test_cross_edge_box_X_axis(
 
70
//      const btVector3 & edge,
 
71
//      const btVector3 & absolute_edge,
 
72
//      const btVector3 & pointa,
 
73
//      const btVector3 & pointb, btVector3 & extend)
 
74
//{
 
75
//
 
76
//      return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,2,1,1,2);
 
77
//}
 
78
//
 
79
//
 
80
//SIMD_FORCE_INLINE bool test_cross_edge_box_Y_axis(
 
81
//      const btVector3 & edge,
 
82
//      const btVector3 & absolute_edge,
 
83
//      const btVector3 & pointa,
 
84
//      const btVector3 & pointb, btVector3 & extend)
 
85
//{
 
86
//
 
87
//      return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,0,2,2,0);
 
88
//}
 
89
//
 
90
//SIMD_FORCE_INLINE bool test_cross_edge_box_Z_axis(
 
91
//      const btVector3 & edge,
 
92
//      const btVector3 & absolute_edge,
 
93
//      const btVector3 & pointa,
 
94
//      const btVector3 & pointb, btVector3 & extend)
 
95
//{
 
96
//
 
97
//      return test_cross_edge_box(edge,absolute_edge,pointa,pointb,extend,1,0,0,1);
 
98
//}
 
99
 
 
100
#define TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,i_dir_0,i_dir_1,i_comp_0,i_comp_1)\
 
101
{\
 
102
        const btScalar dir0 = -edge[i_dir_0];\
 
103
        const btScalar dir1 = edge[i_dir_1];\
 
104
        btScalar pmin = pointa[i_comp_0]*dir0 + pointa[i_comp_1]*dir1;\
 
105
        btScalar pmax = pointb[i_comp_0]*dir0 + pointb[i_comp_1]*dir1;\
 
106
        if(pmin>pmax)\
 
107
        {\
 
108
                GIM_SWAP_NUMBERS(pmin,pmax); \
 
109
        }\
 
110
        const btScalar abs_dir0 = absolute_edge[i_dir_0];\
 
111
        const btScalar abs_dir1 = absolute_edge[i_dir_1];\
 
112
        const btScalar rad = _extend[i_comp_0] * abs_dir0 + _extend[i_comp_1] * abs_dir1;\
 
113
        if(pmin>rad || -rad>pmax) return false;\
 
114
}\
 
115
 
 
116
 
 
117
#define TEST_CROSS_EDGE_BOX_X_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
 
118
{\
 
119
        TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,2,1,1,2);\
 
120
}\
 
121
 
 
122
#define TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
 
123
{\
 
124
        TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,0,2,2,0);\
 
125
}\
 
126
 
 
127
#define TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(edge,absolute_edge,pointa,pointb,_extend)\
 
128
{\
 
129
        TEST_CROSS_EDGE_BOX_MCR(edge,absolute_edge,pointa,pointb,_extend,1,0,0,1);\
 
130
}\
 
131
 
 
132
 
 
133
 
 
134
//!  Class for transforming a model1 to the space of model0
 
135
class GIM_BOX_BOX_TRANSFORM_CACHE
 
136
{
 
137
public:
 
138
    btVector3  m_T1to0;//!< Transforms translation of model1 to model 0
 
139
        btMatrix3x3 m_R1to0;//!< Transforms Rotation of model1 to model 0, equal  to R0' * R1
 
140
        btMatrix3x3 m_AR;//!< Absolute value of m_R1to0
 
141
 
 
142
        SIMD_FORCE_INLINE void calc_absolute_matrix()
 
143
        {
 
144
                static const btVector3 vepsi(1e-6f,1e-6f,1e-6f);
 
145
                m_AR[0] = vepsi + m_R1to0[0].absolute();
 
146
                m_AR[1] = vepsi + m_R1to0[1].absolute();
 
147
                m_AR[2] = vepsi + m_R1to0[2].absolute();
 
148
        }
 
149
 
 
150
        GIM_BOX_BOX_TRANSFORM_CACHE()
 
151
        {
 
152
        }
 
153
 
 
154
 
 
155
        GIM_BOX_BOX_TRANSFORM_CACHE(mat4f  trans1_to_0)
 
156
        {
 
157
                COPY_MATRIX_3X3(m_R1to0,trans1_to_0)
 
158
        MAT_GET_TRANSLATION(trans1_to_0,m_T1to0)
 
159
                calc_absolute_matrix();
 
160
        }
 
161
 
 
162
        //! Calc the transformation relative  1 to 0. Inverts matrics by transposing
 
163
        SIMD_FORCE_INLINE void calc_from_homogenic(const btTransform & trans0,const btTransform & trans1)
 
164
        {
 
165
 
 
166
                m_R1to0 = trans0.getBasis().transpose();
 
167
                m_T1to0 = m_R1to0 * (-trans0.getOrigin());
 
168
 
 
169
                m_T1to0 += m_R1to0*trans1.getOrigin();
 
170
                m_R1to0 *= trans1.getBasis();
 
171
 
 
172
                calc_absolute_matrix();
 
173
        }
 
174
 
 
175
        //! Calcs the full invertion of the matrices. Useful for scaling matrices
 
176
        SIMD_FORCE_INLINE void calc_from_full_invert(const btTransform & trans0,const btTransform & trans1)
 
177
        {
 
178
                m_R1to0 = trans0.getBasis().inverse();
 
179
                m_T1to0 = m_R1to0 * (-trans0.getOrigin());
 
180
 
 
181
                m_T1to0 += m_R1to0*trans1.getOrigin();
 
182
                m_R1to0 *= trans1.getBasis();
 
183
 
 
184
                calc_absolute_matrix();
 
185
        }
 
186
 
 
187
        SIMD_FORCE_INLINE btVector3 transform(const btVector3 & point)
 
188
        {
 
189
                return btVector3(m_R1to0[0].dot(point) + m_T1to0.x(),
 
190
                        m_R1to0[1].dot(point) + m_T1to0.y(),
 
191
                        m_R1to0[2].dot(point) + m_T1to0.z());
 
192
        }
 
193
};
 
194
 
 
195
 
 
196
#define BOX_PLANE_EPSILON 0.000001f
 
197
 
 
198
//! Axis aligned box
 
199
class GIM_AABB
 
200
{
 
201
public:
 
202
        btVector3 m_min;
 
203
        btVector3 m_max;
 
204
 
 
205
        GIM_AABB()
 
206
        {}
 
207
 
 
208
 
 
209
        GIM_AABB(const btVector3 & V1,
 
210
                         const btVector3 & V2,
 
211
                         const btVector3 & V3)
 
212
        {
 
213
                m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
 
214
                m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
 
215
                m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
 
216
 
 
217
                m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
 
218
                m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
 
219
                m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
 
220
        }
 
221
 
 
222
        GIM_AABB(const btVector3 & V1,
 
223
                         const btVector3 & V2,
 
224
                         const btVector3 & V3,
 
225
                         GREAL margin)
 
226
        {
 
227
                m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
 
228
                m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
 
229
                m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
 
230
 
 
231
                m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
 
232
                m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
 
233
                m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
 
234
 
 
235
                m_min[0] -= margin;
 
236
                m_min[1] -= margin;
 
237
                m_min[2] -= margin;
 
238
                m_max[0] += margin;
 
239
                m_max[1] += margin;
 
240
                m_max[2] += margin;
 
241
        }
 
242
 
 
243
        GIM_AABB(const GIM_AABB &other):
 
244
                m_min(other.m_min),m_max(other.m_max)
 
245
        {
 
246
        }
 
247
 
 
248
        GIM_AABB(const GIM_AABB &other,btScalar margin ):
 
249
                m_min(other.m_min),m_max(other.m_max)
 
250
        {
 
251
                m_min[0] -= margin;
 
252
                m_min[1] -= margin;
 
253
                m_min[2] -= margin;
 
254
                m_max[0] += margin;
 
255
                m_max[1] += margin;
 
256
                m_max[2] += margin;
 
257
        }
 
258
 
 
259
        SIMD_FORCE_INLINE void invalidate()
 
260
        {
 
261
                m_min[0] = G_REAL_INFINITY;
 
262
                m_min[1] = G_REAL_INFINITY;
 
263
                m_min[2] = G_REAL_INFINITY;
 
264
                m_max[0] = -G_REAL_INFINITY;
 
265
                m_max[1] = -G_REAL_INFINITY;
 
266
                m_max[2] = -G_REAL_INFINITY;
 
267
        }
 
268
 
 
269
        SIMD_FORCE_INLINE void increment_margin(btScalar margin)
 
270
        {
 
271
                m_min[0] -= margin;
 
272
                m_min[1] -= margin;
 
273
                m_min[2] -= margin;
 
274
                m_max[0] += margin;
 
275
                m_max[1] += margin;
 
276
                m_max[2] += margin;
 
277
        }
 
278
 
 
279
        SIMD_FORCE_INLINE void copy_with_margin(const GIM_AABB &other, btScalar margin)
 
280
        {
 
281
                m_min[0] = other.m_min[0] - margin;
 
282
                m_min[1] = other.m_min[1] - margin;
 
283
                m_min[2] = other.m_min[2] - margin;
 
284
 
 
285
                m_max[0] = other.m_max[0] + margin;
 
286
                m_max[1] = other.m_max[1] + margin;
 
287
                m_max[2] = other.m_max[2] + margin;
 
288
        }
 
289
 
 
290
        template<typename CLASS_POINT>
 
291
        SIMD_FORCE_INLINE void calc_from_triangle(
 
292
                                                        const CLASS_POINT & V1,
 
293
                                                        const CLASS_POINT & V2,
 
294
                                                        const CLASS_POINT & V3)
 
295
        {
 
296
                m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
 
297
                m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
 
298
                m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
 
299
 
 
300
                m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
 
301
                m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
 
302
                m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
 
303
        }
 
304
 
 
305
        template<typename CLASS_POINT>
 
306
        SIMD_FORCE_INLINE void calc_from_triangle_margin(
 
307
                                                        const CLASS_POINT & V1,
 
308
                                                        const CLASS_POINT & V2,
 
309
                                                        const CLASS_POINT & V3, btScalar margin)
 
310
        {
 
311
                m_min[0] = GIM_MIN3(V1[0],V2[0],V3[0]);
 
312
                m_min[1] = GIM_MIN3(V1[1],V2[1],V3[1]);
 
313
                m_min[2] = GIM_MIN3(V1[2],V2[2],V3[2]);
 
314
 
 
315
                m_max[0] = GIM_MAX3(V1[0],V2[0],V3[0]);
 
316
                m_max[1] = GIM_MAX3(V1[1],V2[1],V3[1]);
 
317
                m_max[2] = GIM_MAX3(V1[2],V2[2],V3[2]);
 
318
 
 
319
                m_min[0] -= margin;
 
320
                m_min[1] -= margin;
 
321
                m_min[2] -= margin;
 
322
                m_max[0] += margin;
 
323
                m_max[1] += margin;
 
324
                m_max[2] += margin;
 
325
        }
 
326
 
 
327
        //! Apply a transform to an AABB
 
328
        SIMD_FORCE_INLINE void appy_transform(const btTransform & trans)
 
329
        {
 
330
                btVector3 center = (m_max+m_min)*0.5f;
 
331
                btVector3 extends = m_max - center;
 
332
                // Compute new center
 
333
                center = trans(center);
 
334
 
 
335
                btVector3 textends(extends.dot(trans.getBasis().getRow(0).absolute()),
 
336
                                 extends.dot(trans.getBasis().getRow(1).absolute()),
 
337
                                 extends.dot(trans.getBasis().getRow(2).absolute()));
 
338
 
 
339
                m_min = center - textends;
 
340
                m_max = center + textends;
 
341
        }
 
342
 
 
343
        //! Merges a Box
 
344
        SIMD_FORCE_INLINE void merge(const GIM_AABB & box)
 
345
        {
 
346
                m_min[0] = GIM_MIN(m_min[0],box.m_min[0]);
 
347
                m_min[1] = GIM_MIN(m_min[1],box.m_min[1]);
 
348
                m_min[2] = GIM_MIN(m_min[2],box.m_min[2]);
 
349
 
 
350
                m_max[0] = GIM_MAX(m_max[0],box.m_max[0]);
 
351
                m_max[1] = GIM_MAX(m_max[1],box.m_max[1]);
 
352
                m_max[2] = GIM_MAX(m_max[2],box.m_max[2]);
 
353
        }
 
354
 
 
355
        //! Merges a point
 
356
        template<typename CLASS_POINT>
 
357
        SIMD_FORCE_INLINE void merge_point(const CLASS_POINT & point)
 
358
        {
 
359
                m_min[0] = GIM_MIN(m_min[0],point[0]);
 
360
                m_min[1] = GIM_MIN(m_min[1],point[1]);
 
361
                m_min[2] = GIM_MIN(m_min[2],point[2]);
 
362
 
 
363
                m_max[0] = GIM_MAX(m_max[0],point[0]);
 
364
                m_max[1] = GIM_MAX(m_max[1],point[1]);
 
365
                m_max[2] = GIM_MAX(m_max[2],point[2]);
 
366
        }
 
367
 
 
368
        //! Gets the extend and center
 
369
        SIMD_FORCE_INLINE void get_center_extend(btVector3 & center,btVector3 & extend)  const
 
370
        {
 
371
                center = (m_max+m_min)*0.5f;
 
372
                extend = m_max - center;
 
373
        }
 
374
 
 
375
        //! Finds the intersecting box between this box and the other.
 
376
        SIMD_FORCE_INLINE void find_intersection(const GIM_AABB & other, GIM_AABB & intersection)  const
 
377
        {
 
378
                intersection.m_min[0] = GIM_MAX(other.m_min[0],m_min[0]);
 
379
                intersection.m_min[1] = GIM_MAX(other.m_min[1],m_min[1]);
 
380
                intersection.m_min[2] = GIM_MAX(other.m_min[2],m_min[2]);
 
381
 
 
382
                intersection.m_max[0] = GIM_MIN(other.m_max[0],m_max[0]);
 
383
                intersection.m_max[1] = GIM_MIN(other.m_max[1],m_max[1]);
 
384
                intersection.m_max[2] = GIM_MIN(other.m_max[2],m_max[2]);
 
385
        }
 
386
 
 
387
 
 
388
        SIMD_FORCE_INLINE bool has_collision(const GIM_AABB & other) const
 
389
        {
 
390
                if(m_min[0] > other.m_max[0] ||
 
391
                   m_max[0] < other.m_min[0] ||
 
392
                   m_min[1] > other.m_max[1] ||
 
393
                   m_max[1] < other.m_min[1] ||
 
394
                   m_min[2] > other.m_max[2] ||
 
395
                   m_max[2] < other.m_min[2])
 
396
                {
 
397
                        return false;
 
398
                }
 
399
                return true;
 
400
        }
 
401
 
 
402
        /*! \brief Finds the Ray intersection parameter.
 
403
        \param aabb Aligned box
 
404
        \param vorigin A vec3f with the origin of the ray
 
405
        \param vdir A vec3f with the direction of the ray
 
406
        */
 
407
        SIMD_FORCE_INLINE bool collide_ray(const btVector3 & vorigin,const btVector3 & vdir)
 
408
        {
 
409
                btVector3 extents,center;
 
410
                this->get_center_extend(center,extents);;
 
411
 
 
412
                btScalar Dx = vorigin[0] - center[0];
 
413
                if(GIM_GREATER(Dx, extents[0]) && Dx*vdir[0]>=0.0f)     return false;
 
414
                btScalar Dy = vorigin[1] - center[1];
 
415
                if(GIM_GREATER(Dy, extents[1]) && Dy*vdir[1]>=0.0f)     return false;
 
416
                btScalar Dz = vorigin[2] - center[2];
 
417
                if(GIM_GREATER(Dz, extents[2]) && Dz*vdir[2]>=0.0f)     return false;
 
418
 
 
419
 
 
420
                btScalar f = vdir[1] * Dz - vdir[2] * Dy;
 
421
                if(btFabs(f) > extents[1]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[1])) return false;
 
422
                f = vdir[2] * Dx - vdir[0] * Dz;
 
423
                if(btFabs(f) > extents[0]*btFabs(vdir[2]) + extents[2]*btFabs(vdir[0]))return false;
 
424
                f = vdir[0] * Dy - vdir[1] * Dx;
 
425
                if(btFabs(f) > extents[0]*btFabs(vdir[1]) + extents[1]*btFabs(vdir[0]))return false;
 
426
                return true;
 
427
        }
 
428
 
 
429
 
 
430
        SIMD_FORCE_INLINE void projection_interval(const btVector3 & direction, btScalar &vmin, btScalar &vmax) const
 
431
        {
 
432
                btVector3 center = (m_max+m_min)*0.5f;
 
433
                btVector3 extend = m_max-center;
 
434
 
 
435
                btScalar _fOrigin =  direction.dot(center);
 
436
                btScalar _fMaximumExtent = extend.dot(direction.absolute());
 
437
                vmin = _fOrigin - _fMaximumExtent;
 
438
                vmax = _fOrigin + _fMaximumExtent;
 
439
        }
 
440
 
 
441
        SIMD_FORCE_INLINE ePLANE_INTERSECTION_TYPE plane_classify(const btVector4 &plane) const
 
442
        {
 
443
                btScalar _fmin,_fmax;
 
444
                this->projection_interval(plane,_fmin,_fmax);
 
445
 
 
446
                if(plane[3] > _fmax + BOX_PLANE_EPSILON)
 
447
                {
 
448
                        return G_BACK_PLANE; // 0
 
449
                }
 
450
 
 
451
                if(plane[3]+BOX_PLANE_EPSILON >=_fmin)
 
452
                {
 
453
                        return G_COLLIDE_PLANE; //1
 
454
                }
 
455
                return G_FRONT_PLANE;//2
 
456
        }
 
457
 
 
458
        SIMD_FORCE_INLINE bool overlapping_trans_conservative(const GIM_AABB & box, btTransform & trans1_to_0)
 
459
        {
 
460
                GIM_AABB tbox = box;
 
461
                tbox.appy_transform(trans1_to_0);
 
462
                return has_collision(tbox);
 
463
        }
 
464
 
 
465
        //! transcache is the transformation cache from box to this AABB
 
466
        SIMD_FORCE_INLINE bool overlapping_trans_cache(
 
467
                const GIM_AABB & box,const GIM_BOX_BOX_TRANSFORM_CACHE & transcache, bool fulltest)
 
468
        {
 
469
 
 
470
                //Taken from OPCODE
 
471
                btVector3 ea,eb;//extends
 
472
                btVector3 ca,cb;//extends
 
473
                get_center_extend(ca,ea);
 
474
                box.get_center_extend(cb,eb);
 
475
 
 
476
 
 
477
                btVector3 T;
 
478
                btScalar t,t2;
 
479
                int i;
 
480
 
 
481
                // Class I : A's basis vectors
 
482
                for(i=0;i<3;i++)
 
483
                {
 
484
                        T[i] =  transcache.m_R1to0[i].dot(cb) + transcache.m_T1to0[i] - ca[i];
 
485
                        t = transcache.m_AR[i].dot(eb) + ea[i];
 
486
                        if(GIM_GREATER(T[i], t))        return false;
 
487
                }
 
488
                // Class II : B's basis vectors
 
489
                for(i=0;i<3;i++)
 
490
                {
 
491
                        t = MAT_DOT_COL(transcache.m_R1to0,T,i);
 
492
                        t2 = MAT_DOT_COL(transcache.m_AR,ea,i) + eb[i];
 
493
                        if(GIM_GREATER(t,t2))   return false;
 
494
                }
 
495
                // Class III : 9 cross products
 
496
                if(fulltest)
 
497
                {
 
498
                        int j,m,n,o,p,q,r;
 
499
                        for(i=0;i<3;i++)
 
500
                        {
 
501
                                m = (i+1)%3;
 
502
                                n = (i+2)%3;
 
503
                                o = i==0?1:0;
 
504
                                p = i==2?1:2;
 
505
                                for(j=0;j<3;j++)
 
506
                                {
 
507
                                        q = j==2?1:2;
 
508
                                        r = j==0?1:0;
 
509
                                        t = T[n]*transcache.m_R1to0[m][j] - T[m]*transcache.m_R1to0[n][j];
 
510
                                        t2 = ea[o]*transcache.m_AR[p][j] + ea[p]*transcache.m_AR[o][j] +
 
511
                                                eb[r]*transcache.m_AR[i][q] + eb[q]*transcache.m_AR[i][r];
 
512
                                        if(GIM_GREATER(t,t2))   return false;
 
513
                                }
 
514
                        }
 
515
                }
 
516
                return true;
 
517
        }
 
518
 
 
519
        //! Simple test for planes.
 
520
        SIMD_FORCE_INLINE bool collide_plane(
 
521
                const btVector4 & plane)
 
522
        {
 
523
                ePLANE_INTERSECTION_TYPE classify = plane_classify(plane);
 
524
                return (classify == G_COLLIDE_PLANE);
 
525
        }
 
526
 
 
527
        //! test for a triangle, with edges
 
528
        SIMD_FORCE_INLINE bool collide_triangle_exact(
 
529
                const btVector3 & p1,
 
530
                const btVector3 & p2,
 
531
                const btVector3 & p3,
 
532
                const btVector4 & triangle_plane)
 
533
        {
 
534
                if(!collide_plane(triangle_plane)) return false;
 
535
 
 
536
                btVector3 center,extends;
 
537
                this->get_center_extend(center,extends);
 
538
 
 
539
                const btVector3 v1(p1 - center);
 
540
                const btVector3 v2(p2 - center);
 
541
                const btVector3 v3(p3 - center);
 
542
 
 
543
                //First axis
 
544
                btVector3 diff(v2 - v1);
 
545
                btVector3 abs_diff = diff.absolute();
 
546
                //Test With X axis
 
547
                TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v1,v3,extends);
 
548
                //Test With Y axis
 
549
                TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v1,v3,extends);
 
550
                //Test With Z axis
 
551
                TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v1,v3,extends);
 
552
 
 
553
 
 
554
                diff = v3 - v2;
 
555
                abs_diff = diff.absolute();
 
556
                //Test With X axis
 
557
                TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v2,v1,extends);
 
558
                //Test With Y axis
 
559
                TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v2,v1,extends);
 
560
                //Test With Z axis
 
561
                TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v2,v1,extends);
 
562
 
 
563
                diff = v1 - v3;
 
564
                abs_diff = diff.absolute();
 
565
                //Test With X axis
 
566
                TEST_CROSS_EDGE_BOX_X_AXIS_MCR(diff,abs_diff,v3,v2,extends);
 
567
                //Test With Y axis
 
568
                TEST_CROSS_EDGE_BOX_Y_AXIS_MCR(diff,abs_diff,v3,v2,extends);
 
569
                //Test With Z axis
 
570
                TEST_CROSS_EDGE_BOX_Z_AXIS_MCR(diff,abs_diff,v3,v2,extends);
 
571
 
 
572
                return true;
 
573
        }
 
574
};
 
575
 
 
576
 
 
577
//! Compairison of transformation objects
 
578
SIMD_FORCE_INLINE bool btCompareTransformsEqual(const btTransform & t1,const btTransform & t2)
 
579
{
 
580
        if(!(t1.getOrigin() == t2.getOrigin()) ) return false;
 
581
 
 
582
        if(!(t1.getBasis().getRow(0) == t2.getBasis().getRow(0)) ) return false;
 
583
        if(!(t1.getBasis().getRow(1) == t2.getBasis().getRow(1)) ) return false;
 
584
        if(!(t1.getBasis().getRow(2) == t2.getBasis().getRow(2)) ) return false;
 
585
        return true;
 
586
}
 
587
 
 
588
 
 
589
 
 
590
#endif // GIM_BOX_COLLISION_H_INCLUDED