← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/bullet/src/BulletCollision/Gimpact/gim_box_collision.h

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/bullet/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 Leon Najera
 
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