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 btVector3 margin(getMargin(),getMargin(),getMargin());
23 m_implicitShapeDimensions = (halfExtents * m_localScaling) - margin;
24 m_shapeType = CYLINDER_SHAPE_PROXYTYPE;
25 }
26
27
btCylinderShapeX(const btVector3 & halfExtents)28 btCylinderShapeX::btCylinderShapeX (const btVector3& halfExtents)
29 :btCylinderShape(halfExtents)
30 {
31 m_upAxis = 0;
32
33 }
34
35
btCylinderShapeZ(const btVector3 & halfExtents)36 btCylinderShapeZ::btCylinderShapeZ (const btVector3& halfExtents)
37 :btCylinderShape(halfExtents)
38 {
39 m_upAxis = 2;
40
41 }
42
getAabb(const btTransform & t,btVector3 & aabbMin,btVector3 & aabbMax) const43 void btCylinderShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
44 {
45 btTransformAabb(getHalfExtentsWithoutMargin(),getMargin(),t,aabbMin,aabbMax);
46 }
47
calculateLocalInertia(btScalar mass,btVector3 & inertia) const48 void btCylinderShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
49 {
50
51 //Until Bullet 2.77 a box approximation was used, so uncomment this if you need backwards compatibility
52 //#define USE_BOX_INERTIA_APPROXIMATION 1
53 #ifndef USE_BOX_INERTIA_APPROXIMATION
54
55 /*
56 cylinder is defined as following:
57 *
58 * - principle axis aligned along y by default, radius in x, z-value not used
59 * - for btCylinderShapeX: principle axis aligned along x, radius in y direction, z-value not used
60 * - for btCylinderShapeZ: principle axis aligned along z, radius in x direction, y-value not used
61 *
62 */
63
64 btScalar radius2; // square of cylinder radius
65 btScalar height2; // square of cylinder height
66 btVector3 halfExtents = getHalfExtentsWithMargin(); // get cylinder dimension
67 btScalar div12 = mass / 12.f;
68 btScalar div4 = mass / 4.f;
69 btScalar div2 = mass / 2.f;
70 int idxRadius, idxHeight;
71
72 switch (m_upAxis) // get indices of radius and height of cylinder
73 {
74 case 0: // cylinder is aligned along x
75 idxRadius = 1;
76 idxHeight = 0;
77 break;
78 case 2: // cylinder is aligned along z
79 idxRadius = 0;
80 idxHeight = 2;
81 break;
82 default: // cylinder is aligned along y
83 idxRadius = 0;
84 idxHeight = 1;
85 }
86
87 // calculate squares
88 radius2 = halfExtents[idxRadius] * halfExtents[idxRadius];
89 height2 = btScalar(4.) * halfExtents[idxHeight] * halfExtents[idxHeight];
90
91 // calculate tensor terms
92 btScalar t1 = div12 * height2 + div4 * radius2;
93 btScalar t2 = div2 * radius2;
94
95 switch (m_upAxis) // set diagonal elements of inertia tensor
96 {
97 case 0: // cylinder is aligned along x
98 inertia.setValue(t2,t1,t1);
99 break;
100 case 2: // cylinder is aligned along z
101 inertia.setValue(t1,t1,t2);
102 break;
103 default: // cylinder is aligned along y
104 inertia.setValue(t1,t2,t1);
105 }
106 #else //USE_BOX_INERTIA_APPROXIMATION
107 //approximation of box shape
108 btVector3 halfExtents = getHalfExtentsWithMargin();
109
110 btScalar lx=btScalar(2.)*(halfExtents.x());
111 btScalar ly=btScalar(2.)*(halfExtents.y());
112 btScalar lz=btScalar(2.)*(halfExtents.z());
113
114 inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
115 mass/(btScalar(12.0)) * (lx*lx + lz*lz),
116 mass/(btScalar(12.0)) * (lx*lx + ly*ly));
117 #endif //USE_BOX_INERTIA_APPROXIMATION
118 }
119
120
CylinderLocalSupportX(const btVector3 & halfExtents,const btVector3 & v)121 SIMD_FORCE_INLINE btVector3 CylinderLocalSupportX(const btVector3& halfExtents,const btVector3& v)
122 {
123 const int cylinderUpAxis = 0;
124 const int XX = 1;
125 const int YY = 0;
126 const int ZZ = 2;
127
128 //mapping depends on how cylinder local orientation is
129 // extents of the cylinder is: X,Y is for radius, and Z for height
130
131
132 btScalar radius = halfExtents[XX];
133 btScalar halfHeight = halfExtents[cylinderUpAxis];
134
135
136 btVector3 tmp;
137 btScalar d ;
138
139 btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
140 if (s != btScalar(0.0))
141 {
142 d = radius / s;
143 tmp[XX] = v[XX] * d;
144 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
145 tmp[ZZ] = v[ZZ] * d;
146 return tmp;
147 }
148 else
149 {
150 tmp[XX] = radius;
151 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
152 tmp[ZZ] = btScalar(0.0);
153 return tmp;
154 }
155
156
157 }
158
159
160
161
162
163
CylinderLocalSupportY(const btVector3 & halfExtents,const btVector3 & v)164 inline btVector3 CylinderLocalSupportY(const btVector3& halfExtents,const btVector3& v)
165 {
166
167 const int cylinderUpAxis = 1;
168 const int XX = 0;
169 const int YY = 1;
170 const int ZZ = 2;
171
172
173 btScalar radius = halfExtents[XX];
174 btScalar halfHeight = halfExtents[cylinderUpAxis];
175
176
177 btVector3 tmp;
178 btScalar d ;
179
180 btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
181 if (s != btScalar(0.0))
182 {
183 d = radius / s;
184 tmp[XX] = v[XX] * d;
185 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
186 tmp[ZZ] = v[ZZ] * d;
187 return tmp;
188 }
189 else
190 {
191 tmp[XX] = radius;
192 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
193 tmp[ZZ] = btScalar(0.0);
194 return tmp;
195 }
196
197 }
198
CylinderLocalSupportZ(const btVector3 & halfExtents,const btVector3 & v)199 inline btVector3 CylinderLocalSupportZ(const btVector3& halfExtents,const btVector3& v)
200 {
201 const int cylinderUpAxis = 2;
202 const int XX = 0;
203 const int YY = 2;
204 const int ZZ = 1;
205
206 //mapping depends on how cylinder local orientation is
207 // extents of the cylinder is: X,Y is for radius, and Z for height
208
209
210 btScalar radius = halfExtents[XX];
211 btScalar halfHeight = halfExtents[cylinderUpAxis];
212
213
214 btVector3 tmp;
215 btScalar d ;
216
217 btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
218 if (s != btScalar(0.0))
219 {
220 d = radius / s;
221 tmp[XX] = v[XX] * d;
222 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
223 tmp[ZZ] = v[ZZ] * d;
224 return tmp;
225 }
226 else
227 {
228 tmp[XX] = radius;
229 tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
230 tmp[ZZ] = btScalar(0.0);
231 return tmp;
232 }
233
234
235 }
236
localGetSupportingVertexWithoutMargin(const btVector3 & vec) const237 btVector3 btCylinderShapeX::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
238 {
239 return CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vec);
240 }
241
242
localGetSupportingVertexWithoutMargin(const btVector3 & vec) const243 btVector3 btCylinderShapeZ::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
244 {
245 return CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vec);
246 }
localGetSupportingVertexWithoutMargin(const btVector3 & vec) const247 btVector3 btCylinderShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
248 {
249 return CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vec);
250 }
251
batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 * vectors,btVector3 * supportVerticesOut,int numVectors) const252 void btCylinderShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
253 {
254 for (int i=0;i<numVectors;i++)
255 {
256 supportVerticesOut[i] = CylinderLocalSupportY(getHalfExtentsWithoutMargin(),vectors[i]);
257 }
258 }
259
batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 * vectors,btVector3 * supportVerticesOut,int numVectors) const260 void btCylinderShapeZ::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
261 {
262 for (int i=0;i<numVectors;i++)
263 {
264 supportVerticesOut[i] = CylinderLocalSupportZ(getHalfExtentsWithoutMargin(),vectors[i]);
265 }
266 }
267
268
269
270
batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 * vectors,btVector3 * supportVerticesOut,int numVectors) const271 void btCylinderShapeX::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
272 {
273 for (int i=0;i<numVectors;i++)
274 {
275 supportVerticesOut[i] = CylinderLocalSupportX(getHalfExtentsWithoutMargin(),vectors[i]);
276 }
277 }
278
279
280