6a815cb3113215b6f193fff68c0380eafdd41d40
[blender.git] / extern / bullet / BulletDynamics / CollisionDispatch / ConvexConcaveCollisionAlgorithm.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 "ConvexConcaveCollisionAlgorithm.h"
12 #include "Dynamics/RigidBody.h"
13 #include "CollisionShapes/MultiSphereShape.h"
14 #include "ConstraintSolver/ContactConstraint.h"
15 #include "CollisionShapes/BoxShape.h"
16 #include "ConvexConvexAlgorithm.h"
17 #include "BroadphaseCollision/BroadphaseProxy.h"
18 #include "CollisionShapes/TriangleShape.h"
19 #include "ConstraintSolver/ConstraintSolver.h"
20 #include "ConstraintSolver/ContactSolverInfo.h"
21 #include "CollisionDispatch/ManifoldResult.h"
22 #include "NarrowphaseCollision/RaycastCallback.h"
23 #include "CollisionShapes/TriangleMeshShape.h"
24
25
26 ConvexConcaveCollisionAlgorithm::ConvexConcaveCollisionAlgorithm( const CollisionAlgorithmConstructionInfo& ci,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1)
27 : CollisionAlgorithm(ci),m_convex(*proxy0),m_concave(*proxy1),
28 m_boxTriangleCallback(ci.m_dispatcher,proxy0,proxy1)
29 {
30 }
31
32 ConvexConcaveCollisionAlgorithm::~ConvexConcaveCollisionAlgorithm()
33 {
34 }
35
36
37
38 BoxTriangleCallback::BoxTriangleCallback(Dispatcher*  dispatcher,BroadphaseProxy* proxy0,BroadphaseProxy* proxy1):
39   m_boxProxy(proxy0),m_triangleProxy(*proxy1),m_dispatcher(dispatcher),
40           m_timeStep(0.f),
41           m_stepCount(-1)
42 {
43
44           m_triangleProxy.SetClientObjectType(TRIANGLE_SHAPE_PROXYTYPE);
45
46           //
47           // create the manifold from the dispatcher 'manifold pool'
48           //
49           m_manifoldPtr = m_dispatcher->GetNewManifold(proxy0->m_clientObject,proxy1->m_clientObject);
50
51           ClearCache();
52 }
53
54 BoxTriangleCallback::~BoxTriangleCallback()
55 {
56         ClearCache();
57         m_dispatcher->ReleaseManifold( m_manifoldPtr );
58   
59 }
60   
61
62 void    BoxTriangleCallback::ClearCache()
63 {
64
65         m_manifoldPtr->ClearManifold();
66 };
67
68
69
70 void BoxTriangleCallback::ProcessTriangle(SimdVector3* triangle)
71 {
72  
73
74         RigidBody* triangleBody = (RigidBody*)m_triangleProxy.m_clientObject;
75
76         //aabb filter is already applied!       
77
78         CollisionAlgorithmConstructionInfo ci;
79         ci.m_dispatcher = m_dispatcher;
80
81         ConvexShape* tmp = static_cast<ConvexShape*>(triangleBody->GetCollisionShape());
82
83         if (m_boxProxy->IsConvexShape())
84         {
85                 TriangleShape tm(triangle[0],triangle[1],triangle[2]);  
86                 tm.SetMargin(m_collisionMarginTriangle);
87
88                 RigidBody* triangleBody = (RigidBody* )m_triangleProxy.m_clientObject;
89                 
90                 triangleBody->SetCollisionShape(&tm);
91                 ConvexConvexAlgorithm cvxcvxalgo(m_manifoldPtr,ci,m_boxProxy,&m_triangleProxy);
92                 triangleBody->SetCollisionShape(&tm);
93                 cvxcvxalgo.ProcessCollision(m_boxProxy,&m_triangleProxy,m_timeStep,m_stepCount,m_useContinuous);
94         }
95
96         triangleBody->SetCollisionShape(tmp);
97
98 }
99
100
101
102 void    BoxTriangleCallback::SetTimeStepAndCounters(float timeStep,int stepCount,float collisionMarginTriangle,bool useContinuous)
103 {
104         m_timeStep = timeStep;
105         m_stepCount = stepCount;
106         m_useContinuous = useContinuous;
107         m_collisionMarginTriangle = collisionMarginTriangle;
108
109         //recalc aabbs
110         RigidBody* boxBody = (RigidBody* )m_boxProxy->m_clientObject;
111         RigidBody* triBody = (RigidBody* )m_triangleProxy.m_clientObject;
112
113         SimdTransform boxInTriangleSpace;
114         boxInTriangleSpace = triBody->getCenterOfMassTransform().inverse() * boxBody->getCenterOfMassTransform();
115
116         boxBody->GetCollisionShape()->GetAabb(boxInTriangleSpace,m_aabbMin,m_aabbMax);
117
118         float extraMargin = CONVEX_DISTANCE_MARGIN+0.1f;
119
120         SimdVector3 extra(extraMargin,extraMargin,extraMargin);
121
122         m_aabbMax += extra;
123         m_aabbMin -= extra;
124         
125 }
126
127 void ConvexConcaveCollisionAlgorithm::ClearCache()
128 {
129         m_boxTriangleCallback.ClearCache();
130
131 }
132
133 void ConvexConcaveCollisionAlgorithm::ProcessCollision (BroadphaseProxy* ,BroadphaseProxy* ,float timeStep,int stepCount,bool useContinuous)
134 {
135
136
137         if (m_concave.GetClientObjectType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
138         {
139
140                 RigidBody* convexbody = (RigidBody* )m_convex.m_clientObject;
141                 RigidBody* concavebody = (RigidBody* )m_concave.m_clientObject;
142
143                 TriangleMeshShape* triangleMesh = (TriangleMeshShape*) concavebody->GetCollisionShape();
144                 
145                 if (m_convex.IsConvexShape())
146                 {
147                         float collisionMarginTriangle = triangleMesh->GetMargin();
148                                         
149                         m_boxTriangleCallback.SetTimeStepAndCounters(timeStep,stepCount, collisionMarginTriangle,useContinuous);
150 #ifdef USE_BOX_TRIANGLE
151                         m_boxTriangleCallback.m_manifoldPtr->ClearManifold();
152 #endif
153                         m_boxTriangleCallback.m_manifoldPtr->SetBodies(convexbody,concavebody);         
154
155                         triangleMesh->ProcessAllTriangles( &m_boxTriangleCallback,m_boxTriangleCallback.GetAabbMin(),m_boxTriangleCallback.GetAabbMax());
156                         
157         
158                 }
159
160         }
161
162 }
163
164
165 float ConvexConcaveCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* ,BroadphaseProxy* ,float timeStep,int stepCount)
166 {
167
168         return 1.f;
169
170         //quick approximation using raycast, todo: use proper continuou collision detection
171         RigidBody* convexbody = (RigidBody* )m_convex.m_clientObject;
172         const SimdVector3& from = convexbody->getCenterOfMassPosition();
173                 
174         SimdVector3 radVec(0,0,0);
175         
176         float minradius = 0.05f;
177         float lenSqr = convexbody->getLinearVelocity().length2();
178         if (lenSqr > SIMD_EPSILON)
179         {
180                 radVec = convexbody->getLinearVelocity();
181                 radVec.normalize();
182                 radVec *= minradius;
183         }
184
185         SimdVector3 to = from + radVec + convexbody->getLinearVelocity() * timeStep*1.01f;
186         //only do if the motion exceeds the 'radius'
187
188
189         RaycastCallback raycastCallback(from,to);
190
191         raycastCallback.m_hitFraction = convexbody->m_hitFraction;
192
193         SimdVector3 aabbMin (-1e30f,-1e30f,-1e30f);
194         SimdVector3 aabbMax (SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
195
196         if (m_concave.GetClientObjectType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
197         {
198
199                 RigidBody* concavebody = (RigidBody* )m_concave.m_clientObject;
200
201                 TriangleMeshShape* triangleMesh = (TriangleMeshShape*) concavebody->GetCollisionShape();
202                 
203                 if (triangleMesh)
204                 {
205                         triangleMesh->ProcessAllTriangles(&raycastCallback,aabbMin,aabbMax);
206                 }
207         }
208
209
210         if (raycastCallback.m_hitFraction < convexbody->m_hitFraction)
211         {
212                 convexbody->m_hitFraction = raycastCallback.m_hitFraction;
213                 return raycastCallback.m_hitFraction;
214         }
215
216         return 1.f;
217
218 }