Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/LinearMath/btAabbUtil2.h @ 2427

Last change on this file since 2427 was 2192, checked in by rgrieder, 17 years ago

Reverted all changes of attempt to update physics branch.
  • Property svn:eol-style set to native
File size: 6.6 KB
Line 
1/*
2Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
3
4This software is provided 'as-is', without any express or implied warranty.
5In no event will the authors be held liable for any damages arising from the use of this software.
6Permission is granted to anyone to use this software for any purpose,
7including commercial applications, and to alter it and redistribute it freely,
8subject to the following restrictions:
9
101. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
112. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
123. This notice may not be removed or altered from any source distribution.
13*/
14
15
16
17#ifndef AABB_UTIL2
18#define AABB_UTIL2
19
20#include "btTransform.h"
21#include "btVector3.h"
22#include "btMinMax.h"
23
24SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin,
25                                                                   btVector3& aabbMax,
26                                                                   const btVector3& expansionMin,
27                                                                   const btVector3& expansionMax)
28{
29        aabbMin = aabbMin + expansionMin;
30        aabbMax = aabbMax + expansionMax;
31}
32
33
34/// conservative test for overlap between two aabbs
35SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
36                                                                const btVector3 &aabbMin2, const btVector3 &aabbMax2)
37{
38        bool overlap = true;
39        overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
40        overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
41        overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
42        return overlap;
43}
44
45/// conservative test for overlap between triangle and aabb
46SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices,
47                                                                        const btVector3 &aabbMin, const btVector3 &aabbMax)
48{
49        const btVector3 &p1 = vertices[0];
50        const btVector3 &p2 = vertices[1];
51        const btVector3 &p3 = vertices[2];
52
53        if (btMin(btMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
54        if (btMax(btMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
55
56        if (btMin(btMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
57        if (btMax(btMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
58 
59        if (btMin(btMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
60        if (btMax(btMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
61        return true;
62}
63
64
65SIMD_FORCE_INLINE int   btOutcode(const btVector3& p,const btVector3& halfExtent) 
66{
67        return (p.getX()  < -halfExtent.getX() ? 0x01 : 0x0) |   
68                   (p.getX() >  halfExtent.getX() ? 0x08 : 0x0) |
69                   (p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |   
70                   (p.getY() >  halfExtent.getY() ? 0x10 : 0x0) |
71                   (p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |   
72                   (p.getZ() >  halfExtent.getZ() ? 0x20 : 0x0);
73}
74
75
76SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom,
77                                                                  const btVector3& rayInvDirection,
78                                                                  const unsigned int raySign[3],
79                                                                  const btVector3 bounds[2],
80                                                                  btScalar& tmin,
81                                                                  btScalar lambda_min,
82                                                                  btScalar lambda_max)
83{
84        btScalar tmax, tymin, tymax, tzmin, tzmax;
85        tmin = (bounds[raySign[0]][0] - rayFrom[0]) * rayInvDirection[0];
86        tmax = (bounds[1-raySign[0]][0] - rayFrom[0]) * rayInvDirection[0];
87        tymin = (bounds[raySign[1]][1] - rayFrom[1]) * rayInvDirection[1];
88        tymax = (bounds[1-raySign[1]][1] - rayFrom[1]) * rayInvDirection[1];
89
90        if ( (tmin > tymax) || (tymin > tmax) )
91                return false;
92
93        if (tymin > tmin)
94                tmin = tymin;
95
96        if (tymax < tmax)
97                tmax = tymax;
98
99        tzmin = (bounds[raySign[2]][2] - rayFrom[2]) * rayInvDirection[2];
100        tzmax = (bounds[1-raySign[2]][2] - rayFrom[2]) * rayInvDirection[2];
101
102        if ( (tmin > tzmax) || (tzmin > tmax) )
103                return false;
104        if (tzmin > tmin)
105                tmin = tzmin;
106        if (tzmax < tmax)
107                tmax = tzmax;
108        return ( (tmin < lambda_max) && (tmax > lambda_min) );
109}
110
111SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom, 
112                                                                 const btVector3& rayTo, 
113                                                                 const btVector3& aabbMin, 
114                                                                 const btVector3& aabbMax,
115                                          btScalar& param, btVector3& normal) 
116{
117        btVector3 aabbHalfExtent = (aabbMax-aabbMin)* btScalar(0.5);
118        btVector3 aabbCenter = (aabbMax+aabbMin)* btScalar(0.5);
119        btVector3       source = rayFrom - aabbCenter;
120        btVector3       target = rayTo - aabbCenter;
121        int     sourceOutcode = btOutcode(source,aabbHalfExtent);
122        int targetOutcode = btOutcode(target,aabbHalfExtent);
123        if ((sourceOutcode & targetOutcode) == 0x0)
124        {
125                btScalar lambda_enter = btScalar(0.0);
126                btScalar lambda_exit  = param;
127                btVector3 r = target - source;
128                int i;
129                btScalar        normSign = 1;
130                btVector3       hitNormal(0,0,0);
131                int bit=1;
132
133                for (int j=0;j<2;j++)
134                {
135                        for (i = 0; i != 3; ++i)
136                        {
137                                if (sourceOutcode & bit)
138                                {
139                                        btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
140                                        if (lambda_enter <= lambda)
141                                        {
142                                                lambda_enter = lambda;
143                                                hitNormal.setValue(0,0,0);
144                                                hitNormal[i] = normSign;
145                                        }
146                                }
147                                else if (targetOutcode & bit) 
148                                {
149                                        btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
150                                        btSetMin(lambda_exit, lambda);
151                                }
152                                bit<<=1;
153                        }
154                        normSign = btScalar(-1.);
155                }
156                if (lambda_enter <= lambda_exit)
157                {
158                        param = lambda_enter;
159                        normal = hitNormal;
160                        return true;
161                }
162        }
163        return false;
164}
165
166
167
168SIMD_FORCE_INLINE       void btTransformAabb(const btVector3& halfExtents, btScalar margin,const btTransform& t,btVector3& aabbMinOut,btVector3& aabbMaxOut)
169{
170        btVector3 halfExtentsWithMargin = halfExtents+btVector3(margin,margin,margin);
171        btMatrix3x3 abs_b = t.getBasis().absolute(); 
172        btVector3 center = t.getOrigin();
173        btVector3 extent = btVector3(abs_b[0].dot(halfExtentsWithMargin),
174                   abs_b[1].dot(halfExtentsWithMargin),
175                  abs_b[2].dot(halfExtentsWithMargin));
176        aabbMinOut = center - extent;
177        aabbMaxOut = center + extent;
178}
179
180
181SIMD_FORCE_INLINE       void btTransformAabb(const btVector3& localAabbMin,const btVector3& localAabbMax, btScalar margin,const btTransform& trans,btVector3& aabbMinOut,btVector3& aabbMaxOut)
182{
183                btAssert(localAabbMin.getX() <= localAabbMax.getX());
184                btAssert(localAabbMin.getY() <= localAabbMax.getY());
185                btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
186                btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
187                localHalfExtents+=btVector3(margin,margin,margin);
188
189                btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
190                btMatrix3x3 abs_b = trans.getBasis().absolute(); 
191                btVector3 center = trans(localCenter);
192                btVector3 extent = btVector3(abs_b[0].dot(localHalfExtents),
193                           abs_b[1].dot(localHalfExtents),
194                          abs_b[2].dot(localHalfExtents));
195                aabbMinOut = center-extent;
196                aabbMaxOut = center+extent;
197}
198
199
200#endif
201
202
Note: See TracBrowser for help on using the repository browser.