added physics-debugging
[blender.git] / extern / bullet / BulletDynamics / ConstraintSolver / OdeConstraintSolver.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
12 #include "OdeConstraintSolver.h"
13
14 #include "NarrowPhaseCollision/PersistentManifold.h"
15 #include "Dynamics/RigidBody.h"
16 #include "ContactConstraint.h"
17 #include "Solve2LinearConstraint.h"
18 #include "ContactSolverInfo.h"
19 #include "Dynamics/BU_Joint.h"
20 #include "Dynamics/ContactJoint.h"
21
22 #include "IDebugDraw.h"
23
24 #define USE_SOR_SOLVER
25
26 #include "SorLcp.h"
27
28 #include <math.h>
29 #include <float.h>//FLT_MAX
30 #ifdef WIN32
31 #include <memory.h>
32 #endif
33 #include <string.h>
34 #include <stdio.h>
35
36 #ifdef WIN32
37 #include <malloc.h>
38 #else
39 #include <alloca.h>
40 #endif
41
42 class BU_Joint;
43
44 //see below
45
46
47
48 int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies);
49 void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints,
50                                            RigidBody** bodies,int bodyId0,int bodyId1);
51
52
53
54
55
56 //iterative lcp and penalty method
57 float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal,IDebugDraw* debugDrawer)
58 {
59         m_CurBody = 0;
60         m_CurJoint = 0;
61
62         float cfm = 1e-5f;
63         float erp = 0.2f;
64
65         RigidBody* bodies [128];
66
67         int numBodies = 0;
68         BU_Joint* joints [128*5];
69         int numJoints = 0;
70
71         for (int j=0;j<numManifolds;j++)
72         {
73
74                 int body0=-1,body1=-1;
75
76                 PersistentManifold* manifold = manifoldPtr[j];
77                 if (manifold->GetNumContacts() > 0)
78                 {
79                         body0 = ConvertBody((RigidBody*)manifold->GetBody0(),bodies,numBodies);
80                         body1 = ConvertBody((RigidBody*)manifold->GetBody1(),bodies,numBodies);
81                         ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1,debugDrawer);
82                 }
83         }
84
85         SolveInternal1(cfm,erp,bodies,numBodies,joints,numJoints,infoGlobal);
86
87         return 0.f;
88
89 }
90
91 /////////////////////////////////////////////////////////////////////////////////
92
93
94 typedef SimdScalar dQuaternion[4];
95 #define _R(i,j) R[(i)*4+(j)]
96
97 void dRfromQ1 (dMatrix3 R, const dQuaternion q)
98 {
99   // q = (s,vx,vy,vz)
100   SimdScalar qq1 = 2*q[1]*q[1];
101   SimdScalar qq2 = 2*q[2]*q[2];
102   SimdScalar qq3 = 2*q[3]*q[3];
103   _R(0,0) = 1 - qq2 - qq3;
104   _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]);
105   _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]);
106   _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]);
107   _R(1,1) = 1 - qq1 - qq3;
108   _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]);
109   _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]);
110   _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]);
111   _R(2,2) = 1 - qq1 - qq2;
112 }
113
114
115
116 int OdeConstraintSolver::ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies)
117 {
118         if (!body || (body->getInvMass() == 0.f) )
119         {
120                 return -1;
121         }
122         //first try to find
123         int i,j;
124         for (i=0;i<numBodies;i++)
125         {
126                 if (bodies[i] == body)
127                         return i;
128         }
129         //if not found, create a new body
130         bodies[numBodies++] = body;
131         //convert data
132
133
134         body->m_facc.setValue(0,0,0);
135         body->m_tacc.setValue(0,0,0);
136         
137         //are the indices the same ?
138         for (i=0;i<4;i++)
139         {
140                 for ( j=0;j<3;j++)
141                 {
142                         body->m_invI[i+4*j] = 0.f;
143                         body->m_I[i+4*j] = 0.f;
144                 }
145         }
146         body->m_invI[0+4*0] =   body->getInvInertiaDiagLocal()[0];
147         body->m_invI[1+4*1] =   body->getInvInertiaDiagLocal()[1];
148         body->m_invI[2+4*2] =   body->getInvInertiaDiagLocal()[2];
149
150         body->m_I[0+0*4] = body->getInvInertiaDiagLocal()[0];
151         body->m_I[1+1*4] = body->getInvInertiaDiagLocal()[1];
152         body->m_I[2+2*4] = body->getInvInertiaDiagLocal()[2];
153         
154
155         /*
156         
157         SimdMatrix3x3 invI;
158         invI.setIdentity();
159         invI[0][0] = body->getInvInertiaDiagLocal()[0];
160         invI[1][1] = body->getInvInertiaDiagLocal()[1];
161         invI[2][2] = body->getInvInertiaDiagLocal()[2];
162         SimdMatrix3x3 inertia = invI.inverse();
163
164         for (i=0;i<3;i++)
165         {
166                 for (j=0;j<3;j++)
167                 {
168                         body->m_I[i+4*j] = inertia[i][j];
169                 }
170         }
171         */
172 //      body->m_I[3+0*4] = 0.f;
173 //      body->m_I[3+1*4] = 0.f;
174 //      body->m_I[3+2*4] = 0.f;
175 //      body->m_I[3+3*4] = 0.f;
176         
177         
178         dQuaternion q;
179
180         q[1] = body->getOrientation()[0];
181         q[2] = body->getOrientation()[1];
182         q[3] = body->getOrientation()[2];
183         q[0] = body->getOrientation()[3];
184         
185         dRfromQ1(body->m_R,q);
186         
187         return numBodies-1;
188 }
189
190
191
192
193         
194 #define MAX_JOINTS_1 8192
195
196 static ContactJoint gJointArray[MAX_JOINTS_1];
197
198
199 void OdeConstraintSolver::ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints,
200                                            RigidBody** bodies,int _bodyId0,int _bodyId1,IDebugDraw* debugDrawer)
201 {
202
203
204         manifold->RefreshContactPoints(((RigidBody*)manifold->GetBody0())->getCenterOfMassTransform(),
205                 ((RigidBody*)manifold->GetBody1())->getCenterOfMassTransform());
206
207         int bodyId0 = _bodyId0,bodyId1 = _bodyId1;
208
209         int i,numContacts = manifold->GetNumContacts();
210         
211         bool swapBodies = (bodyId0 < 0);
212
213         
214         RigidBody* body0,*body1;
215
216         if (swapBodies)
217         {
218                 bodyId0 = _bodyId1;
219                 bodyId1 = _bodyId0;
220
221                 body0 = (RigidBody*)manifold->GetBody1();
222                 body1 = (RigidBody*)manifold->GetBody0();
223
224         } else
225         {
226                 body0 = (RigidBody*)manifold->GetBody0();
227                 body1 = (RigidBody*)manifold->GetBody1();
228         }
229
230         assert(bodyId0 >= 0);
231
232         SimdVector3 color(0,1,0);
233         for (i=0;i<numContacts;i++)
234         {
235
236                 if (debugDrawer)
237                 {
238                         debugDrawer->DrawLine(manifold->GetContactPoint(i).m_positionWorldOnA,manifold->GetContactPoint(i).m_positionWorldOnB,color);
239                         debugDrawer->DrawLine(manifold->GetContactPoint(i).m_positionWorldOnA,manifold->GetContactPoint(i).m_positionWorldOnB,color);
240
241                 }
242                 assert (m_CurJoint < MAX_JOINTS_1);
243
244                 if (manifold->GetContactPoint(i).GetDistance() < 0.f)
245                 {
246                         ContactJoint* cont = new (&gJointArray[m_CurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1);
247
248                         cont->node[0].joint = cont;
249                         cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0;
250                         
251                         cont->node[1].joint = cont;
252                         cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0;
253                         
254                         joints[numJoints++] = cont;
255                         for (int i=0;i<6;i++)
256                                 cont->lambda[i] = 0.f;
257
258                         cont->flags = 0;
259                 }
260         }
261
262         //create a new contact constraint
263 };
264