diff options
Diffstat (limited to 'extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp')
-rw-r--r-- | extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp | 256 |
1 files changed, 256 insertions, 0 deletions
diff --git a/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp new file mode 100644 index 00000000000..8dc666d588f --- /dev/null +++ b/extern/bullet/BulletDynamics/ConstraintSolver/OdeConstraintSolver.cpp @@ -0,0 +1,256 @@ +/* + * Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com + * + * Permission to use, copy, modify, distribute and sell this software + * and its documentation for any purpose is hereby granted without fee, + * provided that the above copyright notice appear in all copies. + * Erwin Coumans makes no representations about the suitability + * of this software for any purpose. + * It is provided "as is" without express or implied warranty. +*/ + +#include "OdeConstraintSolver.h" + +#include "NarrowPhaseCollision/PersistentManifold.h" +#include "Dynamics/RigidBody.h" +#include "ContactConstraint.h" +#include "Solve2LinearConstraint.h" +#include "ContactSolverInfo.h" +#include "Dynamics/BU_Joint.h" +#include "Dynamics/ContactJoint.h" + +#define USE_SOR_SOLVER + +#include "SorLcp.h" + +#include <math.h> +#include <float.h>//FLT_MAX +#ifdef WIN32 +#include <memory.h> +#endif +#include <string.h> +#include <stdio.h> + +#ifdef WIN32 +#include <malloc.h> +#else +#include <alloca.h> +#endif + +class BU_Joint; + +//see below + +int gCurBody = 0; +int gCurJoint= 0; + +int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies); +void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints, + RigidBody** bodies,int bodyId0,int bodyId1); + + + + + +//iterative lcp and penalty method +float OdeConstraintSolver::SolveGroup(PersistentManifold** manifoldPtr, int numManifolds,const ContactSolverInfo& infoGlobal) +{ + gCurBody = 0; + gCurJoint = 0; + + float cfm = 1e-5f; + float erp = 0.2f; + + RigidBody* bodies [128]; + + int numBodies = 0; + BU_Joint* joints [128*5]; + int numJoints = 0; + + for (int j=0;j<numManifolds;j++) + { + + int body0=-1,body1=-1; + + PersistentManifold* manifold = manifoldPtr[j]; + if (manifold->GetNumContacts() > 0) + { + body0 = ConvertBody((RigidBody*)manifold->GetBody0(),bodies,numBodies); + body1 = ConvertBody((RigidBody*)manifold->GetBody1(),bodies,numBodies); + ConvertConstraint(manifold,joints,numJoints,bodies,body0,body1); + } + } + + SolveInternal1(cfm,erp,bodies,numBodies,joints,numJoints,infoGlobal); + + return 0.f; + +} + +///////////////////////////////////////////////////////////////////////////////// + + +typedef SimdScalar dQuaternion[4]; +#define _R(i,j) R[(i)*4+(j)] + +void dRfromQ1 (dMatrix3 R, const dQuaternion q) +{ + // q = (s,vx,vy,vz) + SimdScalar qq1 = 2*q[1]*q[1]; + SimdScalar qq2 = 2*q[2]*q[2]; + SimdScalar qq3 = 2*q[3]*q[3]; + _R(0,0) = 1 - qq2 - qq3; + _R(0,1) = 2*(q[1]*q[2] - q[0]*q[3]); + _R(0,2) = 2*(q[1]*q[3] + q[0]*q[2]); + _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]); + _R(1,1) = 1 - qq1 - qq3; + _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]); + _R(2,0) = 2*(q[1]*q[3] - q[0]*q[2]); + _R(2,1) = 2*(q[2]*q[3] + q[0]*q[1]); + _R(2,2) = 1 - qq1 - qq2; +} + + + +int ConvertBody(RigidBody* body,RigidBody** bodies,int& numBodies) +{ + if (!body || (body->getInvMass() == 0.f) ) + { + return -1; + } + //first try to find + int i,j; + for (i=0;i<numBodies;i++) + { + if (bodies[i] == body) + return i; + } + //if not found, create a new body + bodies[numBodies++] = body; + //convert data + + + body->m_facc.setValue(0,0,0); + body->m_tacc.setValue(0,0,0); + + //are the indices the same ? + for (i=0;i<4;i++) + { + for ( j=0;j<3;j++) + { + body->m_invI[i+4*j] = 0.f; + body->m_I[i+4*j] = 0.f; + } + } + body->m_invI[0+4*0] = body->getInvInertiaDiagLocal()[0]; + body->m_invI[1+4*1] = body->getInvInertiaDiagLocal()[1]; + body->m_invI[2+4*2] = body->getInvInertiaDiagLocal()[2]; + + body->m_I[0+0*4] = body->getInvInertiaDiagLocal()[0]; + body->m_I[1+1*4] = body->getInvInertiaDiagLocal()[1]; + body->m_I[2+2*4] = body->getInvInertiaDiagLocal()[2]; + + + /* + + SimdMatrix3x3 invI; + invI.setIdentity(); + invI[0][0] = body->getInvInertiaDiagLocal()[0]; + invI[1][1] = body->getInvInertiaDiagLocal()[1]; + invI[2][2] = body->getInvInertiaDiagLocal()[2]; + SimdMatrix3x3 inertia = invI.inverse(); + + for (i=0;i<3;i++) + { + for (j=0;j<3;j++) + { + body->m_I[i+4*j] = inertia[i][j]; + } + } + */ +// body->m_I[3+0*4] = 0.f; +// body->m_I[3+1*4] = 0.f; +// body->m_I[3+2*4] = 0.f; +// body->m_I[3+3*4] = 0.f; + + + dQuaternion q; + + q[1] = body->getOrientation()[0]; + q[2] = body->getOrientation()[1]; + q[3] = body->getOrientation()[2]; + q[0] = body->getOrientation()[3]; + + dRfromQ1(body->m_R,q); + + return numBodies-1; +} + + + + + +#define MAX_JOINTS_1 8192 + +static ContactJoint gJointArray[MAX_JOINTS_1]; + + +void ConvertConstraint(PersistentManifold* manifold,BU_Joint** joints,int& numJoints, + RigidBody** bodies,int _bodyId0,int _bodyId1) +{ + + + manifold->RefreshContactPoints(((RigidBody*)manifold->GetBody0())->getCenterOfMassTransform(), + ((RigidBody*)manifold->GetBody1())->getCenterOfMassTransform()); + + int bodyId0 = _bodyId0,bodyId1 = _bodyId1; + + int i,numContacts = manifold->GetNumContacts(); + + bool swapBodies = (bodyId0 < 0); + + + RigidBody* body0,*body1; + + if (swapBodies) + { + bodyId0 = _bodyId1; + bodyId1 = _bodyId0; + + body0 = (RigidBody*)manifold->GetBody1(); + body1 = (RigidBody*)manifold->GetBody0(); + + } else + { + body0 = (RigidBody*)manifold->GetBody0(); + body1 = (RigidBody*)manifold->GetBody1(); + } + + assert(bodyId0 >= 0); + + for (i=0;i<numContacts;i++) + { + + assert (gCurJoint < MAX_JOINTS_1); + + if (manifold->GetContactPoint(i).GetDistance() < 0.f) + { + ContactJoint* cont = new (&gJointArray[gCurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1); + + cont->node[0].joint = cont; + cont->node[0].body = bodyId0 >= 0 ? bodies[bodyId0] : 0; + + cont->node[1].joint = cont; + cont->node[1].body = bodyId1 >= 0 ? bodies[bodyId1] : 0; + + joints[numJoints++] = cont; + for (int i=0;i<6;i++) + cont->lambda[i] = 0.f; + + cont->flags = 0; + } + } + + //create a new contact constraint +}; + |