0c20f78c1dfae69cea2d257f5637106606170924
[blender.git] / extern / bullet / Bullet / CollisionShapes / TriangleMeshShape.cpp
1 /*
2  * Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
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 const SimdVector3& TriangleMeshShape::getLocalScaling() const
104 {
105         return m_meshInterface->getScaling();
106 }
107
108
109
110 void    TriangleMeshShape::ProcessAllTriangles(TriangleCallback* callback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
111 {
112
113         SimdVector3 meshScaling = m_meshInterface->getScaling();
114         int numtotalphysicsverts = 0;
115         int part,graphicssubparts = m_meshInterface->getNumSubParts();
116         for (part=0;part<graphicssubparts ;part++)
117         {
118                 unsigned char * vertexbase;
119                 unsigned char * indexbase;
120                 int indexstride;
121                 PHY_ScalarType type;
122                 PHY_ScalarType gfxindextype;
123                 int stride,numverts,numtriangles;
124                 m_meshInterface->getLockedVertexIndexBase(&vertexbase,numverts,type,stride,&indexbase,indexstride,numtriangles,gfxindextype,part);
125                 numtotalphysicsverts+=numtriangles*3; //upper bound
126
127         
128                 int gfxindex;
129                 SimdVector3 triangle[3];
130
131                 for (gfxindex=0;gfxindex<numtriangles;gfxindex++)
132                 {
133                 
134                         int     graphicsindex=0;
135
136                         for (int j=2;j>=0;j--)
137                         {
138                                 ASSERT(gfxindextype == PHY_INTEGER);
139                                 int* gfxbase = (int*)(indexbase+gfxindex*indexstride);
140                                 graphicsindex = gfxbase[j];
141                                 float* graphicsbase = (float*)(vertexbase+graphicsindex*stride);
142
143                                 triangle[j] = SimdVector3(
144                                         graphicsbase[0]*meshScaling.getX(),
145                                         graphicsbase[1]*meshScaling.getY(),
146                                         graphicsbase[2]*meshScaling.getZ());
147                         }
148
149                         if (TestTriangleAgainstAabb2(&triangle[0],aabbMin,aabbMax))
150                         {
151                                 //check aabb in triangle-space, before doing this
152                                 callback->ProcessTriangle(triangle);
153         
154                         }
155                         
156                 }
157                 
158                 m_meshInterface->unLockVertexBase(part);
159         }
160
161
162 }
163
164
165
166
167
168 void    TriangleMeshShape::CalculateLocalInertia(SimdScalar mass,SimdVector3& inertia)
169 {
170         //moving concave objects not supported
171         assert(0);
172         inertia.setValue(0.f,0.f,0.f);
173 }
174
175
176 SimdVector3 TriangleMeshShape::LocalGetSupportingVertex(const SimdVector3& vec) const
177 {
178         SimdVector3 supportVertex;
179
180         SimdTransform ident;
181         ident.setIdentity();
182
183         SupportVertexCallback supportCallback(vec,ident);
184
185         SimdVector3 aabbMax(1e30f,1e30f,1e30f);
186         
187         ProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
188                 
189         supportVertex = supportCallback.GetSupportVertexLocal();
190
191         return supportVertex;
192 }