more physics work, tuning parameters, hooking up friction etc.
[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         if (m_concave.GetClientObjectType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
137         {
138
139                 RigidBody* convexbody = (RigidBody* )m_convex.m_clientObject;
140                 RigidBody* concavebody = (RigidBody* )m_concave.m_clientObject;
141
142                 TriangleMeshShape* triangleMesh = (TriangleMeshShape*) concavebody->GetCollisionShape();
143                 
144                 if (m_convex.IsConvexShape())
145                 {
146                         float collisionMarginTriangle = triangleMesh->GetMargin();
147                                         
148                         m_boxTriangleCallback.SetTimeStepAndCounters(timeStep,stepCount, collisionMarginTriangle,useContinuous);
149 #ifdef USE_BOX_TRIANGLE
150                         m_boxTriangleCallback.m_manifoldPtr->ClearManifold();
151 #endif
152                         m_boxTriangleCallback.m_manifoldPtr->SetBodies(convexbody,concavebody);         
153
154                         triangleMesh->ProcessAllTriangles( &m_boxTriangleCallback,m_boxTriangleCallback.GetAabbMin(),m_boxTriangleCallback.GetAabbMax());
155                         
156         
157                 }
158
159         }
160
161 }
162
163
164 float ConvexConcaveCollisionAlgorithm::CalculateTimeOfImpact(BroadphaseProxy* ,BroadphaseProxy* ,float timeStep,int stepCount)
165 {
166
167         return 1.f;
168
169         //quick approximation using raycast, todo: use proper continuou collision detection
170         RigidBody* convexbody = (RigidBody* )m_convex.m_clientObject;
171         const SimdVector3& from = convexbody->getCenterOfMassPosition();
172                 
173         SimdVector3 radVec(0,0,0);
174         
175         float minradius = 0.05f;
176         float lenSqr = convexbody->getLinearVelocity().length2();
177         if (lenSqr > SIMD_EPSILON)
178         {
179                 radVec = convexbody->getLinearVelocity();
180                 radVec.normalize();
181                 radVec *= minradius;
182         }
183
184         SimdVector3 to = from + radVec + convexbody->getLinearVelocity() * timeStep*1.01f;
185         //only do if the motion exceeds the 'radius'
186
187
188         RaycastCallback raycastCallback(from,to);
189
190         raycastCallback.m_hitFraction = convexbody->m_hitFraction;
191
192         SimdVector3 aabbMin (-1e30f,-1e30f,-1e30f);
193         SimdVector3 aabbMax (SIMD_INFINITY,SIMD_INFINITY,SIMD_INFINITY);
194
195         if (m_concave.GetClientObjectType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
196         {
197
198                 RigidBody* concavebody = (RigidBody* )m_concave.m_clientObject;
199
200                 TriangleMeshShape* triangleMesh = (TriangleMeshShape*) concavebody->GetCollisionShape();
201                 
202                 if (triangleMesh)
203                 {
204                         triangleMesh->ProcessAllTriangles(&raycastCallback,aabbMin,aabbMax);
205                 }
206         }
207
208
209         if (raycastCallback.m_hitFraction < convexbody->m_hitFraction)
210         {
211                 convexbody->m_hitFraction = raycastCallback.m_hitFraction;
212                 return raycastCallback.m_hitFraction;
213         }
214
215         return 1.f;
216
217 }