1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2009 Erwin Coumans http://bulletphysics.org
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 #include "btCylinderShape.h"
17
btCylinderShape(const btVector3 & halfExtents)18 btCylinderShape::btCylinderShape (const btVector3& halfExtents)
19 :btConvexInternalShape(),
20 m_upAxis(1)
21 {
22 setSafeMargin(halfExtents);
23
24 btVector3 margin(getMargin(),getMargin(),getMargin());
25 m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin;
26 m_shapeType = CYLINDER_SHAPE_PROXYTYPE;
27 }
28
29
btCylinderShapeX(const btVector3 & halfExtents)30 btCylinderShapeX::btCylinderShapeX (const btVector3& halfExtents)
31 :btCylinderShape(halfExtents)
32 {
33 m_upAxis = 0;
34
35 }
36
37
btCylinderShapeZ(const btVector3 & halfExtents)38 btCylinderShapeZ::btCylinderShapeZ (const btVector3& halfExtents)
39 :btCylinderShape(halfExtents)
40 {
41 m_upAxis = 2;
42
43 }
44
getAabb(const btTransform & t,btVector3 & aabbMin,btVector3 & aabbMax) const45 void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
46 {
47 btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
48 }
49
calculateLocalInertia(btScalar mass,btVector3 & inertia) const50 void btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
51 {
52
53 //Until Bullet 2.77 a box approximation was used, so uncomment this if you need backwards compatibility
54 //#define USE_BOX_INERTIA_APPROXIMATION 1
55 #ifndef USE_BOX_INERTIA_APPROXIMATION
56
57 /*
58 cylinder is defined as following:
59 *
60 * - principle axis aligned along y by default, radius in x, z-value not used
61 * - for btCylinderShapeX: principle axis aligned along x, radius in y direction, z-value not used
62 * - for btCylinderShapeZ: principle axis aligned along z, radius in x direction, y-value not used
63 *
64 */
65
66 btScalar radius2; // square of cylinder radius
67 btScalar height2; // square of cylinder height
68 btVector3 halfExtents = getHalfExtentsWithMargin(); // get cylinder dimension
69 btScalar div12 = mass / 12.f;
70 btScalar div4 = mass / 4.f;
71 btScalar div2 = mass / 2.f;
72 int idxRadius, idxHeight;
73
74 switch (m_upAxis) // get indices of radius and height of cylinder
75 {
76 case 0: // cylinder is aligned along x
77 idxRadius = 1;
78 idxHeight = 0;
79 break;
80 case 2: // cylinder is aligned along z
81 idxRadius = 0;
82 idxHeight = 2;
83 break;
84 default: // cylinder is aligned along y
85 idxRadius = 0;
86 idxHeight = 1;
87 }
88
89 // calculate squares
90 radius2 = halfExtents[idxRadius] * halfExtents[idxRadius];
91 height2 = btScalar(4.) * halfExtents[idxHeight] * halfExtents[idxHeight];
92
93 // calculate tensor terms
94 btScalar t1 = div12 * height2 + div4 * radius2;
95 btScalar t2 = div2 * radius2;
96
97 switch (m_upAxis) // set diagonal elements of inertia tensor
98 {
99 case 0: // cylinder is aligned along x
100 inertia.setValue(t2,t1,t1);
101 break;
102 case 2: // cylinder is aligned along z
103 inertia.setValue(t1,t1,t2);
104 break;
105 default: // cylinder is aligned along y
106 inertia.setValue(t1,t2,t1);
107 }
108 #else //USE_BOX_INERTIA_APPROXIMATION
109 //approximation of box shape
110 btVector3 halfExtents = getHalfExtentsWithMargin();
111
112 btScalar lx=btScalar(2.)*(halfExtents.x());
113 btScalar ly=btScalar(2.)*(halfExtents.y());
114 btScalar lz=btScalar(2.)*(halfExtents.z());
115
116 inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
117 mass/(btScalar(12.0)) * (lx*lx + lz*lz),
118 mass/(btScalar(12.0)) * (lx*lx + ly*ly));
119 #endif //USE_BOX_INERTIA_APPROXIMATION
120 }
121
122
CylinderLocalSupportX(const btVector3 & halfExtents,const btVector3 & v)123 SIMD_FORCE_INLINE btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v)
124 {
125 const int cylinderUpAxis = 0;
126 const int XX = 1;
127 const int YY = 0;
128 const int ZZ = 2;
129
130 //mapping depends on how cylinder local orientation is
131 // extents of the cylinder is: X,Y is for radius, and Z for height
132
133
134 btScalar radius = halfExtents[XX];
135 btScalar halfHeight = halfExtents[cylinderUpAxis];
136
137
138 btVector3 tmp;
139 btScalar d ;
140
141 btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
142 if (s != btScalar(0.0))
143 {
144 d = radius / s;
145 tmp[XX] = v[XX] * d;
146 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
147 tmp[ZZ] = v[ZZ] * d;
148 return tmp;
149 }
150 else
151 {
152 tmp[XX] = radius;
153 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
154 tmp[ZZ] = btScalar(0.0);
155 return tmp;
156 }
157
158
159 }
160
161
162
163
164
165
CylinderLocalSupportY(const btVector3 & halfExtents,const btVector3 & v)166 inline btVector3 CylinderLocalSupportY(const btVector3& halfExtents,const btVector3& v)
167 {
168
169 const int cylinderUpAxis = 1;
170 const int XX = 0;
171 const int YY = 1;
172 const int ZZ = 2;
173
174
175 btScalar radius = halfExtents[XX];
176 btScalar halfHeight = halfExtents[cylinderUpAxis];
177
178
179 btVector3 tmp;
180 btScalar d ;
181
182 btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
183 if (s != btScalar(0.0))
184 {
185 d = radius / s;
186 tmp[XX] = v[XX] * d;
187 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
188 tmp[ZZ] = v[ZZ] * d;
189 return tmp;
190 }
191 else
192 {
193 tmp[XX] = radius;
194 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
195 tmp[ZZ] = btScalar(0.0);
196 return tmp;
197 }
198
199 }
200
CylinderLocalSupportZ(const btVector3 & halfExtents,const btVector3 & v)201 inline btVector3 CylinderLocalSupportZ(const btVector3& halfExtents,const btVector3& v)
202 {
203 const int cylinderUpAxis = 2;
204 const int XX = 0;
205 const int YY = 2;
206 const int ZZ = 1;
207
208 //mapping depends on how cylinder local orientation is
209 // extents of the cylinder is: X,Y is for radius, and Z for height
210
211
212 btScalar radius = halfExtents[XX];
213 btScalar halfHeight = halfExtents[cylinderUpAxis];
214
215
216 btVector3 tmp;
217 btScalar d ;
218
219 btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
220 if (s != btScalar(0.0))
221 {
222 d = radius / s;
223 tmp[XX] = v[XX] * d;
224 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
225 tmp[ZZ] = v[ZZ] * d;
226 return tmp;
227 }
228 else
229 {
230 tmp[XX] = radius;
231 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
232 tmp[ZZ] = btScalar(0.0);
233 return tmp;
234 }
235
236
237 }
238
localGetSupportingVertexWithoutMargin(const btVector3 & vec) const239 btVector3 btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
240 {
241 return CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vec);
242 }
243
244
localGetSupportingVertexWithoutMargin(const btVector3 & vec) const245 btVector3 btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
246 {
247 return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vec);
248 }
localGetSupportingVertexWithoutMargin(const btVector3 & vec) const249 btVector3 btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
250 {
251 return CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vec);
252 }
253
batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 * vectors,btVector3 * supportVerticesOut,int numVectors) const254 void btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
255 {
256 for (int i=0;i<numVectors;i++)
257 {
258 supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vectors[i]);
259 }
260 }
261
batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 * vectors,btVector3 * supportVerticesOut,int numVectors) const262 void btCylinderShapeZ::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
263 {
264 for (int i=0;i<numVectors;i++)
265 {
266 supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vectors[i]);
267 }
268 }
269
270
271
272
batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 * vectors,btVector3 * supportVerticesOut,int numVectors) const273 void btCylinderShapeX::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
274 {
275 for (int i=0;i<numVectors;i++)
276 {
277 supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vectors[i]);
278 }
279 }
280
281
282