Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / BulletCollision / CollisionShapes / btMultiSphereShape.cpp
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 #if defined (_WIN32) || defined (__i386__)
17 #define BT_USE_SSE_IN_API
18 #endif
19
20 #include "btMultiSphereShape.h"
21 #include "BulletCollision/CollisionShapes/btCollisionMargin.h"
22 #include "LinearMath/btQuaternion.h"
23 #include "LinearMath/btSerializer.h"
24
25 btMultiSphereShape::btMultiSphereShape (const btVector3* positions,const btScalar* radi,int numSpheres)
26 :btConvexInternalAabbCachingShape ()
27 {
28         m_shapeType = MULTI_SPHERE_SHAPE_PROXYTYPE;
29         //btScalar startMargin = btScalar(BT_LARGE_FLOAT);
30
31         m_localPositionArray.resize(numSpheres);
32         m_radiArray.resize(numSpheres);
33         for (int i=0;i<numSpheres;i++)
34         {
35                 m_localPositionArray[i] = positions[i];
36                 m_radiArray[i] = radi[i];
37                 
38         }
39
40         recalcLocalAabb();
41
42 }
43
44 #ifndef MIN
45         #define MIN( _a, _b)    ((_a) < (_b) ? (_a) : (_b))
46 #endif
47  btVector3      btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
48 {
49         btVector3 supVec(0,0,0);
50
51         btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
52
53
54         btVector3 vec = vec0;
55         btScalar lenSqr = vec.length2();
56         if (lenSqr < (SIMD_EPSILON*SIMD_EPSILON))
57         {
58                 vec.setValue(1,0,0);
59         } else
60         {
61                 btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
62                 vec *= rlen;
63         }
64
65         btVector3 vtx;
66         btScalar newDot;
67
68         const btVector3* pos = &m_localPositionArray[0];
69         const btScalar* rad = &m_radiArray[0];
70         int numSpheres = m_localPositionArray.size();
71
72         for( int k = 0; k < numSpheres; k+= 128 )
73         {
74                 btVector3 temp[128];
75                 int inner_count = MIN( numSpheres - k, 128 );
76         for( long i = 0; i < inner_count; i++ )
77         {
78             temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
79             pos++;
80             rad++;
81         }
82         long i = vec.maxDot( temp, inner_count, newDot);
83         if( newDot > maxDot )
84                 {
85                         maxDot = newDot;
86                         supVec = temp[i];
87                 }
88     }
89
90         return supVec;
91
92 }
93
94  void   btMultiSphereShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
95 {
96
97         for (int j=0;j<numVectors;j++)
98         {
99                 btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
100
101                 const btVector3& vec = vectors[j];
102
103                 btVector3 vtx;
104                 btScalar newDot;
105
106                 const btVector3* pos = &m_localPositionArray[0];
107                 const btScalar* rad = &m_radiArray[0];
108                 int numSpheres = m_localPositionArray.size();
109
110         for( int k = 0; k < numSpheres; k+= 128 )
111         {
112             btVector3 temp[128];
113             int inner_count = MIN( numSpheres - k, 128 );
114             for( long i = 0; i < inner_count; i++ )
115             {
116                 temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
117                 pos++;
118                 rad++;
119             }
120             long i = vec.maxDot( temp, inner_count, newDot);
121             if( newDot > maxDot )
122             {
123                 maxDot = newDot;
124                 supportVerticesOut[j] = temp[i];
125             }
126         }
127         
128         }
129 }
130
131
132
133
134
135
136
137
138 void    btMultiSphereShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
139 {
140         //as an approximation, take the inertia of the box that bounds the spheres
141
142         btVector3 localAabbMin,localAabbMax;
143         getCachedLocalAabb(localAabbMin,localAabbMax);
144         btVector3 halfExtents = (localAabbMax-localAabbMin)*btScalar(0.5);
145
146         btScalar lx=btScalar(2.)*(halfExtents.x());
147         btScalar ly=btScalar(2.)*(halfExtents.y());
148         btScalar lz=btScalar(2.)*(halfExtents.z());
149
150         inertia.setValue(mass/(btScalar(12.0)) * (ly*ly + lz*lz),
151                                         mass/(btScalar(12.0)) * (lx*lx + lz*lz),
152                                         mass/(btScalar(12.0)) * (lx*lx + ly*ly));
153
154 }
155
156
157 ///fills the dataBuffer and returns the struct name (and 0 on failure)
158 const char*     btMultiSphereShape::serialize(void* dataBuffer, btSerializer* serializer) const
159 {
160         btMultiSphereShapeData* shapeData = (btMultiSphereShapeData*) dataBuffer;
161         btConvexInternalShape::serialize(&shapeData->m_convexInternalShapeData, serializer);
162
163         int numElem = m_localPositionArray.size();
164         shapeData->m_localPositionArrayPtr = numElem ? (btPositionAndRadius*)serializer->getUniquePointer((void*)&m_localPositionArray[0]):  0;
165         
166         shapeData->m_localPositionArraySize = numElem;
167         if (numElem)
168         {
169                 btChunk* chunk = serializer->allocate(sizeof(btPositionAndRadius),numElem);
170                 btPositionAndRadius* memPtr = (btPositionAndRadius*)chunk->m_oldPtr;
171                 for (int i=0;i<numElem;i++,memPtr++)
172                 {
173                         m_localPositionArray[i].serializeFloat(memPtr->m_pos);
174                         memPtr->m_radius = float(m_radiArray[i]);
175                 }
176                 serializer->finalizeChunk(chunk,"btPositionAndRadius",BT_ARRAY_CODE,(void*)&m_localPositionArray[0]);
177         }
178         
179         return "btMultiSphereShapeData";
180 }
181
182