1 /*
2 ***CHRONO***
3 Bullet Continuous Collision Detection and Physics Library
4 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
5
6 This software is provided 'as-is', without any express or implied warranty.
7 In no event will the authors be held liable for any damages arising from the use of this software.
8 Permission is granted to anyone to use this software for any purpose,
9 including commercial applications, and to alter it and redistribute it freely,
10 subject to the following restrictions:
11
12 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.
13 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
14 3. This notice may not be removed or altered from any source distribution.
15 */
16
17 #include <cmath>
18
19 #include "btBarrelShape.h"
20
21 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
22 #include "LinearMath/btQuaternion.h"
23
btBarrelShape(btScalar sY_low,btScalar sY_high,btScalar sR_vert,btScalar sR_hor,btScalar sR_offset)24 btBarrelShape::btBarrelShape(btScalar sY_low, btScalar sY_high, btScalar sR_vert, btScalar sR_hor, btScalar sR_offset )
25 {
26 Y_low = sY_low;
27 Y_high = sY_high;
28 R_vert = sR_vert;
29 R_hor = sR_hor;
30 R_offset = sR_offset;
31 m_shapeType = BARREL_SHAPE_PROXYTYPE;
32 }
33
34 #include <stdio.h>
localGetSupportingVertexWithoutMargin(const btVector3 & vec0) const35 btVector3 btBarrelShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
36 {
37 btVector3 supVec(0,0,0);
38 btVector3 supVecD;
39
40 // suppport point on the lathed ellipse?
41 btScalar pY = vec0.y();
42 btScalar pR = std::sqrt(vec0.z()*vec0.z() + vec0.x()*vec0.x());
43 btScalar pH = pR;
44 btScalar dX = vec0.x()/pR;
45 btScalar dZ = vec0.z()/pR;
46 btScalar tpar = std::atan((pY*R_vert)/(pH*R_hor));
47 btScalar sY = R_vert * std::sin(tpar);
48 btScalar sH = R_hor * std::cos(tpar);
49 btScalar sR = sH + R_offset;
50 btScalar sX = dX * sR;
51 btScalar sZ = dZ * sR;
52 supVec.setValue(sX,sY,sZ);
53 btScalar len = supVec.length();
54
55 // support point on the top disc?
56 if ((std::abs(Y_high) < R_vert) && (supVec.y() > Y_high)) {
57 btScalar R_high_ellips = R_hor * std::sqrt(btScalar(1) - std::pow(Y_high / R_vert, btScalar(2)));
58 btScalar R_high = R_high_ellips + R_offset;
59 btScalar rad_ratio = pR / R_high;
60 supVecD.setValue(vec0.x() / rad_ratio, Y_high, vec0.z() / rad_ratio);
61 supVec = supVecD;
62 }
63 // support point on the bottom disc?
64 if ((std::abs(Y_low) < R_vert) && (supVec.y() < Y_low)) {
65 btScalar R_low_ellips = R_hor * std::sqrt(btScalar(1) - std::pow(Y_low / R_vert, btScalar(2)));
66 btScalar R_low = R_low_ellips + R_offset;
67 btScalar rad_ratio = pR / R_low;
68 supVecD.setValue(vec0.x() / rad_ratio, Y_low, vec0.z() / rad_ratio);
69 supVec = supVecD;
70 }
71
72 return supVec;
73 }
74
batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3 * vectors,btVector3 * supportVerticesOut,int numVectors) const75 void btBarrelShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
76 {
77 printf("NOT SUPPORTED!! \n");
78 }
79
80
calculateLocalInertia(btScalar mass,btVector3 & inertia) const81 void btBarrelShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
82 {
83 //***TO DO***
84 //as an approximation, take the inertia of the box that bounds the barrell
85
86 btTransform ident;
87 ident.setIdentity();
88
89 btVector3 halfExtents;
90
91 halfExtents.setValue((R_hor+R_offset),
92 (R_vert),
93 (R_hor+R_offset));
94
95 btScalar margin = CONVEX_DISTANCE_MARGIN;
96
97 btScalar lx=btScalar(2.)*(halfExtents[0]+margin);
98 btScalar ly=btScalar(2.)*(halfExtents[1]+margin);
99 btScalar lz=btScalar(2.)*(halfExtents[2]+margin);
100 const btScalar x2 = lx*lx;
101 const btScalar y2 = ly*ly;
102 const btScalar z2 = lz*lz;
103 const btScalar scaledmass = mass * btScalar(.08333333);
104
105 inertia[0] = scaledmass * (y2+z2);
106 inertia[1] = scaledmass * (x2+z2);
107 inertia[2] = scaledmass * (x2+y2);
108 }
109
110
getAabb(const btTransform & t,btVector3 & aabbMin,btVector3 & aabbMax) const111 void btBarrelShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
112 {
113 btVector3 halfExtents;
114 halfExtents.setValue((R_hor+R_offset),
115 (R_vert),
116 (R_hor+R_offset));
117
118 btMatrix3x3 abs_b = t.getBasis().absolute();
119 btVector3 center = t.getOrigin();
120 btVector3 extent = btVector3(abs_b[0].dot(halfExtents),
121 abs_b[1].dot(halfExtents),
122 abs_b[2].dot(halfExtents));
123 extent += btVector3(getMargin(),getMargin(),getMargin());
124
125 aabbMin = center - extent;
126 aabbMax = center + extent;
127
128 }
129
130
131