c09599f39b17db10228ac45638c3a711dfd29880
[blender.git] / extern / bullet / Bullet / CollisionShapes / TriangleMeshShape.cpp
1 /*
2  * Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com
3  *
4  * Permission to use, copy, modify, distribute and sell this software
5  * and its documentation for any purpose is hereby granted without fee,
6  * provided that the above copyright notice appear in all copies.
7  * Erwin Coumans makes no representations about the suitability 
8  * of this software for any purpose.  
9  * It is provided "as is" without express or implied warranty.
10 */
11 #include "TriangleMeshShape.h"
12 #include "SimdVector3.h"
13 #include "SimdQuaternion.h"
14 #include "StridingMeshInterface.h"
15 #include "AabbUtil2.h"
16 #include "NarrowPhaseCollision/CollisionMargin.h"
17
18 #include "stdio.h"
19
20 TriangleMeshShape::TriangleMeshShape(StridingMeshInterface* meshInterface)
21 : m_meshInterface(meshInterface),
22 m_collisionMargin(CONVEX_DISTANCE_MARGIN)
23 {
24 }
25
26 TriangleMeshShape::~TriangleMeshShape()
27 {
28                 
29 }
30
31
32
33
34 void TriangleMeshShape::GetAabb(const SimdTransform& trans,SimdVector3& aabbMin,SimdVector3& aabbMax) const
35 {
36
37         for (int i=0;i<3;i++)
38         {
39                 SimdVector3 vec(0.f,0.f,0.f);
40                 vec[i] = 1.f;
41                 SimdVector3 tmp = trans(LocalGetSupportingVertex(vec*trans.getBasis()));
42                 aabbMax[i] = tmp[i]+m_collisionMargin;
43                 vec[i] = -1.f;
44                 tmp = trans(LocalGetSupportingVertex(vec*trans.getBasis()));
45                 aabbMin[i] = tmp[i]-m_collisionMargin;
46         }
47 }
48
49
50 TriangleCallback::~TriangleCallback()
51 {
52
53 }
54
55 class SupportVertexCallback : public TriangleCallback
56 {
57
58         SimdVector3 m_supportVertexLocal;
59 public:
60
61         SimdTransform   m_worldTrans;
62         SimdScalar m_maxDot;
63         SimdVector3 m_supportVecLocal;
64
65         SupportVertexCallback(const SimdVector3& supportVecWorld,const SimdTransform& trans)
66                 : m_supportVertexLocal(0.f,0.f,0.f), m_worldTrans(trans) ,m_maxDot(-1e30f)
67                 
68         {
69                 m_supportVecLocal = supportVecWorld * m_worldTrans.getBasis();
70         }
71
72         virtual void ProcessTriangle( SimdVector3* triangle)
73         {
74                 for (int i=0;i<3;i++)
75                 {
76                         SimdScalar dot = m_supportVecLocal.dot(triangle[i]);
77                         if (dot > m_maxDot)
78                         {
79                                 m_maxDot = dot;
80                                 m_supportVertexLocal = triangle[i];
81                         }
82                 }
83         }
84
85         SimdVector3 GetSupportVertexWorldSpace()
86         {
87                 return m_worldTrans(m_supportVertexLocal);
88         }
89
90         SimdVector3     GetSupportVertexLocal()
91         {
92                 return m_supportVertexLocal;
93         }
94
95 };
96
97         
98 void TriangleMeshShape::setLocalScaling(const SimdVector3& scaling)
99 {
100         m_meshInterface->setScaling(scaling);
101 }
102
103 void    TriangleMeshShape::ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
104 {
105
106         SimdVector3 meshScaling = m_meshInterface->getScaling();
107         int numtotalphysicsverts = 0;
108         int part,graphicssubparts = m_meshInterface->getNumSubParts();
109         for (part=0;part<graphicssubparts ;part++)
110         {
111                 unsigned char * vertexbase;
112                 unsigned char * indexbase;
113                 int indexstride;
114                 PHY_ScalarType type;
115                 PHY_ScalarType gfxindextype;
116                 int stride,numverts,numtriangles;
117                 m_meshInterface->getLockedVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part);
118                 numtotalphysicsverts+=numtriangles*3; //upper bound
119
120         
121                 int gfxindex;
122                 SimdVector3 triangle[3];
123
124                 for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
125                 {
126                 
127                         int     graphicsindex=0;
128
129                         for (int j=2;j>=0;j--)
130                         {
131                                 ASSERT(gfxindextype == PHY_INTEGER);
132                                 int* gfxbase = (int*)(indexbase+gfxindex*indexstride);
133                                 graphicsindex = gfxbase[j];
134                                 float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
135
136                                 triangle[j] = SimdVector3(
137                                         graphicsbase[0]*meshScaling.getX(),
138                                         graphicsbase[1]*meshScaling.getY(),
139                                         graphicsbase[2]*meshScaling.getZ());
140                         }
141
142                         if (TestTriangleAgainstAabb2(&triangle[0],aabbMin,aabbMax))
143                         {
144                                 //check aabb in triangle-space, before doing this
145                                 callback->ProcessTriangle(triangle);
146         
147                         }
148                         
149                 }
150                 
151                 m_meshInterface->unLockVertexBase(part);
152         }
153
154
155 }
156
157
158
159
160
161 void    TriangleMeshShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
162 {
163         //moving concave objects not supported
164         assert(0);
165         inertia.setValue(0.f,0.f,0.f);
166 }
167
168
169 SimdVector3 TriangleMeshShape::LocalGetSupportingVertex(const SimdVector3& vec) const
170 {
171         SimdVector3 supportVertex;
172
173         SimdTransform ident;
174         ident.setIdentity();
175
176         SupportVertexCallback supportCallback(vec,ident);
177
178         SimdVector3 aabbMax(1e30f,1e30f,1e30f);
179         
180         ProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
181                 
182         supportVertex = supportCallback.GetSupportVertexLocal();
183
184         return supportVertex;
185 }