Back to index

supertuxkart  0.5+dfsg1
btOdeContactJoint.cpp
Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 #include "btOdeContactJoint.h"
00016 #include "btOdeSolverBody.h"
00017 #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
00018 
00019 
00020 //this constant needs to be set up so different solvers give 'similar' results
00021 #define FRICTION_CONSTANT 120.f
00022 
00023 
00024 btOdeContactJoint::btOdeContactJoint(btPersistentManifold* manifold,int index,bool swap,btOdeSolverBody* body0,btOdeSolverBody* body1)
00025 :m_manifold(manifold),
00026 m_index(index),
00027 m_swapBodies(swap),
00028 m_body0(body0),
00029 m_body1(body1)
00030 {
00031 }
00032 
00033 int m_numRows = 3;
00034 
00035 
00036 void btOdeContactJoint::GetInfo1(Info1 *info)
00037 {
00038        info->m = m_numRows;
00039        //friction adds another 2...
00040        
00041        info->nub = 0;
00042 }
00043 
00044 #define dCROSS(a,op,b,c) \
00045   (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \
00046   (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \
00047   (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]);
00048 
00049 #define M_SQRT12 btScalar(0.7071067811865475244008443621048490)
00050 
00051 #define dRecipSqrt(x) ((float)(1.0f/btSqrt(float(x))))         /* reciprocal square root */
00052 
00053 
00054 
00055 void dPlaneSpace1 (const dVector3 n, dVector3 p, dVector3 q)
00056 {
00057   if (btFabs(n[2]) > M_SQRT12) {
00058     // choose p in y-z plane
00059     btScalar a = n[1]*n[1] + n[2]*n[2];
00060     btScalar k = dRecipSqrt (a);
00061     p[0] = 0;
00062     p[1] = -n[2]*k;
00063     p[2] = n[1]*k;
00064     // set q = n x p
00065     q[0] = a*k;
00066     q[1] = -n[0]*p[2];
00067     q[2] = n[0]*p[1];
00068   }
00069   else {
00070     // choose p in x-y plane
00071     btScalar a = n[0]*n[0] + n[1]*n[1];
00072     btScalar k = dRecipSqrt (a);
00073     p[0] = -n[1]*k;
00074     p[1] = n[0]*k;
00075     p[2] = 0;
00076     // set q = n x p
00077     q[0] = -n[2]*p[1];
00078     q[1] = n[2]*p[0];
00079     q[2] = a*k;
00080   }
00081 }
00082 
00083 
00084 
00085 void btOdeContactJoint::GetInfo2(Info2 *info)
00086 {
00087        
00088        int s = info->rowskip;
00089        int s2 = 2*s;
00090        
00091        float swapFactor = m_swapBodies ? -1.f : 1.f;
00092        
00093        // get normal, with sign adjusted for body1/body2 polarity
00094        dVector3 normal;
00095        
00096        
00097        btManifoldPoint& point = m_manifold->getContactPoint(m_index);
00098        
00099        normal[0] = swapFactor*point.m_normalWorldOnB.x();
00100        normal[1] = swapFactor*point.m_normalWorldOnB.y();
00101        normal[2] = swapFactor*point.m_normalWorldOnB.z();
00102        normal[3] = 0;       // @@@ hmmm
00103        
00104        assert(m_body0);
00105        //     if (GetBody0())
00106        btVector3 relativePositionA;
00107        {
00108               relativePositionA = point.getPositionWorldOnA() - m_body0->m_centerOfMassPosition;
00109               dVector3 c1;
00110               c1[0] = relativePositionA.x();
00111               c1[1] = relativePositionA.y();
00112               c1[2] = relativePositionA.z();
00113               
00114               // set jacobian for normal
00115               info->J1l[0] = normal[0];
00116               info->J1l[1] = normal[1];
00117               info->J1l[2] = normal[2];
00118               dCROSS (info->J1a,=,c1,normal);
00119               
00120        }
00121 
00122        btVector3 relativePositionB(0,0,0);
00123        if (m_body1)
00124        {
00125               //            if (GetBody1())
00126               
00127               {
00128                      dVector3 c2;
00129                      btVector3     posBody1 = m_body1 ? m_body1->m_centerOfMassPosition : btVector3(0,0,0);
00130                      relativePositionB = point.getPositionWorldOnB() - posBody1;
00131                      
00132                      //                   for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] -
00133                      //                                   j->node[1].body->pos[i];
00134                      c2[0] = relativePositionB.x();
00135                      c2[1] = relativePositionB.y();
00136                      c2[2] = relativePositionB.z();
00137                      
00138                      info->J2l[0] = -normal[0];
00139                      info->J2l[1] = -normal[1];
00140                      info->J2l[2] = -normal[2];
00141                      dCROSS (info->J2a,= -,c2,normal);
00142               }
00143        }
00144        
00145        btScalar k = info->fps * info->erp;
00146        
00147        float depth = -point.getDistance();
00148 //     if (depth < 0.f)
00149 //            depth = 0.f;
00150        
00151        info->c[0] = k * depth;
00152        //float maxvel = .2f;
00153 
00154 //     if (info->c[0] > maxvel)
00155 //            info->c[0] = maxvel;
00156 
00157 
00158        //can override it, not necessary
00159 //     info->cfm[0] = 0.f;
00160 //     info->cfm[1] = 0.f;
00161 //     info->cfm[2] = 0.f;
00162        
00163        
00164        
00165        // set LCP limits for normal
00166        info->lo[0] = 0;
00167        info->hi[0] = 1e30f;//dInfinity;
00168        info->lo[1] = 0;
00169        info->hi[1] = 0.f;
00170        info->lo[2] = 0.f;
00171        info->hi[2] = 0.f;
00172 
00173 #define DO_THE_FRICTION_2
00174 #ifdef DO_THE_FRICTION_2
00175        // now do jacobian for tangential forces
00176        dVector3 t1,t2;      // two vectors tangential to normal
00177        
00178        dVector3 c1;
00179        c1[0] = relativePositionA.x();
00180        c1[1] = relativePositionA.y();
00181        c1[2] = relativePositionA.z();
00182        
00183        dVector3 c2;
00184        c2[0] = relativePositionB.x();
00185        c2[1] = relativePositionB.y();
00186        c2[2] = relativePositionB.z();
00187        
00188        //combined friction is available in the contact point
00189        float friction = 0.25;//FRICTION_CONSTANT ;//* m_body0->m_friction * m_body1->m_friction;
00190        
00191        // first friction direction
00192        if (m_numRows >= 2) 
00193        {
00194               
00195               
00196               
00197               dPlaneSpace1 (normal,t1,t2);
00198               
00199               info->J1l[s+0] = t1[0];
00200               info->J1l[s+1] = t1[1];
00201               info->J1l[s+2] = t1[2];
00202               dCROSS (info->J1a+s,=,c1,t1);
00203               if (1) { //j->node[1].body) {
00204                      info->J2l[s+0] = -t1[0];
00205                      info->J2l[s+1] = -t1[1];
00206                      info->J2l[s+2] = -t1[2];
00207                      dCROSS (info->J2a+s,= -,c2,t1);
00208               }
00209               // set right hand side
00210               if (0) {//j->contact.surface.mode & dContactMotion1) {
00211                      //info->c[1] = j->contact.surface.motion1;
00212               }
00213               // set LCP bounds and friction index. this depends on the approximation
00214               // mode
00215               //1e30f
00216               
00217               
00218               info->lo[1] = -friction;//-j->contact.surface.mu;
00219               info->hi[1] = friction;//j->contact.surface.mu;
00220               if (1)//j->contact.surface.mode & dContactApprox1_1) 
00221                      info->findex[1] = 0;
00222               
00223               // set slip (constraint force mixing)
00224               if (0)//j->contact.surface.mode & dContactSlip1)
00225               {
00226                      //     info->cfm[1] = j->contact.surface.slip1;
00227               } else
00228               {
00229                      //info->cfm[1] = 0.f;
00230               }
00231        }
00232        
00233        // second friction direction
00234        if (m_numRows >= 3) {
00235               info->J1l[s2+0] = t2[0];
00236               info->J1l[s2+1] = t2[1];
00237               info->J1l[s2+2] = t2[2];
00238               dCROSS (info->J1a+s2,=,c1,t2);
00239               if (1) { //j->node[1].body) {
00240                      info->J2l[s2+0] = -t2[0];
00241                      info->J2l[s2+1] = -t2[1];
00242                      info->J2l[s2+2] = -t2[2];
00243                      dCROSS (info->J2a+s2,= -,c2,t2);
00244               }
00245 
00246               // set right hand side
00247               if (0){//j->contact.surface.mode & dContactMotion2) {
00248                      //info->c[2] = j->contact.surface.motion2;
00249               }
00250               // set LCP bounds and friction index. this depends on the approximation
00251               // mode
00252               if (0){//j->contact.surface.mode & dContactMu2) {
00253                      //info->lo[2] = -j->contact.surface.mu2;
00254                      //info->hi[2] = j->contact.surface.mu2;
00255               }
00256               else {
00257                      info->lo[2] = -friction;
00258                      info->hi[2] = friction;
00259               }
00260               if (0)//j->contact.surface.mode & dContactApprox1_2) 
00261                      
00262               {
00263                      info->findex[2] = 0;
00264               }
00265               // set slip (constraint force mixing)
00266               if (0) //j->contact.surface.mode & dContactSlip2)
00267                      
00268               {
00269                      //info->cfm[2] = j->contact.surface.slip2;
00270                      
00271               }
00272        }
00273        
00274 #endif //DO_THE_FRICTION_2
00275        
00276 }
00277