From a9080f29849012baa84939f64a53f7f2627c891c Mon Sep 17 00:00:00 2001 From: "erwin.coumans" Date: Sun, 30 Mar 2008 20:51:49 +0000 Subject: [PATCH] removed quickstep from Extras folder, into Bullet core as optional constraint solver. Re-distributed under the Zlib license with permission from Russel L. Smith. --- Extras/quickstep/Jamfile | 20 - Extras/quickstep/OdeConstraintSolver.cpp | 405 ------------- Extras/quickstep/OdeConstraintSolver.h | 106 ---- Extras/quickstep/OdeContactJoint.cpp | 277 --------- Extras/quickstep/OdeContactJoint.h | 50 -- Extras/quickstep/OdeJoint.cpp | 25 - Extras/quickstep/OdeJoint.h | 94 ---- Extras/quickstep/OdeMacros.h | 216 ------- Extras/quickstep/OdeSolverBody.cpp | 0 Extras/quickstep/OdeSolverBody.h | 48 -- Extras/quickstep/OdeTypedJoint.cpp | 451 --------------- Extras/quickstep/OdeTypedJoint.h | 111 ---- Extras/quickstep/SorLcp.cpp | 686 ----------------------- Extras/quickstep/SorLcp.h | 116 ---- 14 files changed, 2605 deletions(-) delete mode 100644 Extras/quickstep/Jamfile delete mode 100644 Extras/quickstep/OdeConstraintSolver.cpp delete mode 100644 Extras/quickstep/OdeConstraintSolver.h delete mode 100644 Extras/quickstep/OdeContactJoint.cpp delete mode 100644 Extras/quickstep/OdeContactJoint.h delete mode 100644 Extras/quickstep/OdeJoint.cpp delete mode 100644 Extras/quickstep/OdeJoint.h delete mode 100644 Extras/quickstep/OdeMacros.h delete mode 100644 Extras/quickstep/OdeSolverBody.cpp delete mode 100644 Extras/quickstep/OdeSolverBody.h delete mode 100644 Extras/quickstep/OdeTypedJoint.cpp delete mode 100644 Extras/quickstep/OdeTypedJoint.h delete mode 100644 Extras/quickstep/SorLcp.cpp delete mode 100644 Extras/quickstep/SorLcp.h diff --git a/Extras/quickstep/Jamfile b/Extras/quickstep/Jamfile deleted file mode 100644 index 1b6d9adce..000000000 --- a/Extras/quickstep/Jamfile +++ /dev/null @@ -1,20 +0,0 @@ -SubDir TOP Extras quickstep ; - -#this is a bad global -#IncludeDir Extras/quickstep ; - -Library quickstep : [ Wildcard *.h *.cpp ] : noinstall ; -#internal header path to compile quickstep -CFlags quickstep : [ FIncludes $(TOP)/Extras/quickstep ] ; - -#expose header include path for apps that depend on quickstep -#QUICKSTEP.CFLAGS = [ FIncludes $(TOP)/Extras/quickstep ] ; - -#same for msvcgen -MsvcGenConfig QUICKSTEP.INCDIRS : $(TOP)/Extras/quickstep ; - -#for the include paths -LibDepends quickstep : bulletdynamics ; - - -#InstallHeader [ Wildcard *.h ] : quickstep ; diff --git a/Extras/quickstep/OdeConstraintSolver.cpp b/Extras/quickstep/OdeConstraintSolver.cpp deleted file mode 100644 index 925d3c48f..000000000 --- a/Extras/quickstep/OdeConstraintSolver.cpp +++ /dev/null @@ -1,405 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -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. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - - - -#include "OdeConstraintSolver.h" - -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "BulletDynamics/ConstraintSolver/btContactConstraint.h" -#include "BulletDynamics/ConstraintSolver/btSolve2LinearConstraint.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" -#include "OdeJoint.h" -#include "OdeContactJoint.h" -#include "OdeTypedJoint.h" -#include "OdeSolverBody.h" -#include -#include "LinearMath/btQuickprof.h" - -#include "LinearMath/btIDebugDraw.h" - -#define USE_SOR_SOLVER - -#include "SorLcp.h" - -#include -#include //FLT_MAX -#ifdef WIN32 -#include -#endif -#include -#include - -#if defined (WIN32) -#include -#else -#if defined (__FreeBSD__) -#include -#else -#include -#endif -#endif - -class BU_Joint; - -//see below -//to bridge with ODE quickstep, we make a temp copy of the rigidbodies in each simultion island - - -OdeConstraintSolver::OdeConstraintSolver(): - m_cfm(0.f),//1e-5f), - m_erp(0.4f) -{ -} - - -//iterative lcp and penalty method -btScalar OdeConstraintSolver::solveGroup(btCollisionObject** bodies,int numBulletBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher) -{ - - m_CurBody = 0; - m_CurJoint = 0; - m_CurTypedJoint = 0; - int j; - - int max_contacts = 0; /// should be 4 //Remotion - for ( j=0;jgetNumContacts() > max_contacts) max_contacts = manifold->getNumContacts(); - } - //if(max_contacts > 4) - // printf(" max_contacts > 4"); - - int numBodies = 0; - m_odeBodies.clear(); - m_odeBodies.reserve(numBulletBodies + 1); //??? - // OdeSolverBody* odeBodies [ODE_MAX_SOLVER_BODIES]; - - int numJoints = 0; - m_joints.clear(); - m_joints.reserve(numManifolds * max_contacts + 4 + numConstraints + 1); //??? - // BU_Joint* joints [ODE_MAX_SOLVER_JOINTS*2]; - - m_SolverBodyArray.resize(numBulletBodies + 1); - m_JointArray.resize(numManifolds * max_contacts + 4); - m_TypedJointArray.resize(numConstraints + 1); - - - //capture contacts - int body0=-1,body1=-1; - for (j=0;jgetNumContacts() > 0) - { - body0 = ConvertBody((btRigidBody*)manifold->getBody0(),m_odeBodies,numBodies); - body1 = ConvertBody((btRigidBody*)manifold->getBody1(),m_odeBodies,numBodies); - ConvertConstraint(manifold,m_joints,numJoints,m_odeBodies,body0,body1,debugDrawer); - } - } - - //capture constraints - for (j=0;jgetRigidBodyA(),m_odeBodies,numBodies); - body1 = ConvertBody((btRigidBody*)&typedconstraint->getRigidBodyB(),m_odeBodies,numBodies); - ConvertTypedConstraint(typedconstraint,m_joints,numJoints,m_odeBodies,body0,body1,debugDrawer); - } - //if(numBodies > numBulletBodies) - // printf(" numBodies > numBulletBodies"); - //if(numJoints > numManifolds * 4 + numConstraints) - // printf(" numJoints > numManifolds * 4 + numConstraints"); - - - m_SorLcpSolver.SolveInternal1(m_cfm,m_erp,m_odeBodies,numBodies,m_joints,numJoints,infoGlobal,stackAlloc); ///do - - //write back resulting velocities - for (int i=0;im_invMass) - { - m_odeBodies[i]->m_originalBody->setLinearVelocity(m_odeBodies[i]->m_linearVelocity); - m_odeBodies[i]->m_originalBody->setAngularVelocity(m_odeBodies[i]->m_angularVelocity); - } - } - - - /// Remotion, just free all this here - m_odeBodies.clear(); - m_joints.clear(); - - m_SolverBodyArray.clear(); - m_JointArray.clear(); - m_TypedJointArray.clear(); - - return 0.f; - -} - -///////////////////////////////////////////////////////////////////////////////// - - -typedef btScalar dQuaternion[4]; -#define _R(i,j) R[(i)*4+(j)] - -void dRfromQ1 (dMatrix3 R, const dQuaternion q) -{ - // q = (s,vx,vy,vz) - btScalar qq1 = 2.f*q[1]*q[1]; - btScalar qq2 = 2.f*q[2]*q[2]; - btScalar qq3 = 2.f*q[3]*q[3]; - _R(0,0) = 1.f - 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(0,3) = 0.f; - - _R(1,0) = 2*(q[1]*q[2] + q[0]*q[3]); - _R(1,1) = 1.f - qq1 - qq3; - _R(1,2) = 2*(q[2]*q[3] - q[0]*q[1]); - _R(1,3) = 0.f; - - _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.f - qq1 - qq2; - _R(2,3) = 0.f; - -} - - - -//int OdeConstraintSolver::ConvertBody(btRigidBody* orgBody,OdeSolverBody** bodies,int& numBodies) -int OdeConstraintSolver::ConvertBody(btRigidBody* orgBody,btAlignedObjectArray< OdeSolverBody*> &bodies,int& numBodies) -{ - assert(orgBody); - if (!orgBody || (orgBody->getInvMass() == 0.f) ) - { - return -1; - } - - if (orgBody->getCompanionId()>=0) - { - return orgBody->getCompanionId(); - } - //first try to find - int i,j; - - //if not found, create a new body - // OdeSolverBody* body = bodies[numBodies] = &gSolverBodyArray[numBodies]; - OdeSolverBody* body = &m_SolverBodyArray[numBodies]; - bodies.push_back(body); // Remotion 10.10.07: - - orgBody->setCompanionId(numBodies); - - numBodies++; - - body->m_originalBody = orgBody; - - body->m_facc.setValue(0,0,0,0); - body->m_tacc.setValue(0,0,0,0); - - body->m_linearVelocity = orgBody->getLinearVelocity(); - body->m_angularVelocity = orgBody->getAngularVelocity(); - body->m_invMass = orgBody->getInvMass(); - body->m_centerOfMassPosition = orgBody->getCenterOfMassPosition(); - body->m_friction = orgBody->getFriction(); - - //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] = orgBody->getInvInertiaDiagLocal().x(); - body->m_invI[1+4*1] = orgBody->getInvInertiaDiagLocal().y(); - body->m_invI[2+4*2] = orgBody->getInvInertiaDiagLocal().z(); - - body->m_I[0+0*4] = 1.f/orgBody->getInvInertiaDiagLocal().x(); - body->m_I[1+1*4] = 1.f/orgBody->getInvInertiaDiagLocal().y(); - body->m_I[2+2*4] = 1.f/orgBody->getInvInertiaDiagLocal().z(); - - - - - dQuaternion q; - - q[1] = orgBody->getOrientation().x(); - q[2] = orgBody->getOrientation().y(); - q[3] = orgBody->getOrientation().z(); - q[0] = orgBody->getOrientation().w(); - - dRfromQ1(body->m_R,q); - - return numBodies-1; -} - - - - - - - - - -void OdeConstraintSolver::ConvertConstraint(btPersistentManifold* manifold, - btAlignedObjectArray &joints,int& numJoints, - const btAlignedObjectArray< OdeSolverBody*> &bodies, - int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer) -{ - - - manifold->refreshContactPoints(((btRigidBody*)manifold->getBody0())->getCenterOfMassTransform(), - ((btRigidBody*)manifold->getBody1())->getCenterOfMassTransform()); - - int bodyId0 = _bodyId0,bodyId1 = _bodyId1; - - int i,numContacts = manifold->getNumContacts(); - - bool swapBodies = (bodyId0 < 0); - - - OdeSolverBody* body0,*body1; - - if (swapBodies) - { - bodyId0 = _bodyId1; - bodyId1 = _bodyId0; - - body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1(); - body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0(); - - } - else - { - body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0(); - body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1(); - } - - assert(bodyId0 >= 0); - - btVector3 color(0,1,0); - for (i=0;igetContactPoint(i); - - debugDrawer->drawContactPoint( - cp.m_positionWorldOnB, - cp.m_normalWorldOnB, - cp.getDistance(), - cp.getLifeTime(), - color); - - } - //assert (m_CurJoint < ODE_MAX_SOLVER_JOINTS); - -// if (manifold->getContactPoint(i).getDistance() < 0.0f) - { - - ContactJoint* cont = new (&m_JointArray[m_CurJoint++]) ContactJoint( manifold ,i, swapBodies,body0,body1); - //ContactJoint* cont = new (&gJointArray[m_CurJoint++]) 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; - joints.push_back(cont); // Remotion 10.10.07: - numJoints++; - - for (int i=0;i<6;i++) - cont->lambda[i] = 0.f; - - cont->flags = 0; - } - } - - //create a new contact constraint -} - -void OdeConstraintSolver::ConvertTypedConstraint( - btTypedConstraint * constraint, - btAlignedObjectArray &joints,int& numJoints, - const btAlignedObjectArray< OdeSolverBody*> &bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer) -{ - - int bodyId0 = _bodyId0,bodyId1 = _bodyId1; - bool swapBodies = (bodyId0 < 0); - - - OdeSolverBody* body0,*body1; - - if (swapBodies) - { - bodyId0 = _bodyId1; - bodyId1 = _bodyId0; - - body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody1(); - body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody0(); - - } - else - { - body0 = bodyId0>=0 ? bodies[bodyId0] : 0;//(btRigidBody*)manifold->getBody0(); - body1 = bodyId1>=0 ? bodies[bodyId1] : 0;//(btRigidBody*)manifold->getBody1(); - } - - assert(bodyId0 >= 0); - - - //assert (m_CurTypedJoint < ODE_MAX_SOLVER_JOINTS); - - - OdeTypedJoint * cont = NULL; - - // Determine constraint type - int joint_type = constraint->getConstraintType(); - switch(joint_type) - { - case POINT2POINT_CONSTRAINT_TYPE: - case D6_CONSTRAINT_TYPE: - cont = new (&m_TypedJointArray[m_CurTypedJoint ++]) OdeTypedJoint(constraint,0, swapBodies,body0,body1); - //cont = new (&gTypedJointArray[m_CurTypedJoint ++]) OdeTypedJoint(constraint,0, swapBodies,body0,body1); - break; - - }; - - if(cont) - { - 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; - joints.push_back(cont); // Remotion 10.10.07: - numJoints++; - - for (int i=0;i<6;i++) - cont->lambda[i] = 0.f; - - cont->flags = 0; - } - -} diff --git a/Extras/quickstep/OdeConstraintSolver.h b/Extras/quickstep/OdeConstraintSolver.h deleted file mode 100644 index 1d246ab11..000000000 --- a/Extras/quickstep/OdeConstraintSolver.h +++ /dev/null @@ -1,106 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -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. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef ODE_CONSTRAINT_SOLVER_H -#define ODE_CONSTRAINT_SOLVER_H - -#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" - -#include "LinearMath/btAlignedObjectArray.h" -#include "OdeContactJoint.h" -#include "OdeTypedJoint.h" -#include "OdeSolverBody.h" -#include "SorLcp.h" - -class btRigidBody; -struct OdeSolverBody; -class BU_Joint; - -/// OdeConstraintSolver is one of the available solvers for Bullet dynamics framework -/// It uses the the unmodified version of quickstep solver from the open dynamics project -class OdeConstraintSolver : public btConstraintSolver -{ -private: - int m_CurBody; - int m_CurJoint; - int m_CurTypedJoint; - - float m_cfm; - float m_erp; - - SorLcpSolver m_SorLcpSolver; - - btAlignedObjectArray m_odeBodies; - btAlignedObjectArray m_joints; - - btAlignedObjectArray m_SolverBodyArray; - btAlignedObjectArray m_JointArray; - btAlignedObjectArray m_TypedJointArray; - - -private: - int ConvertBody(btRigidBody* body,btAlignedObjectArray< OdeSolverBody*> &bodies,int& numBodies); - void ConvertConstraint(btPersistentManifold* manifold, - btAlignedObjectArray &joints,int& numJoints, - const btAlignedObjectArray< OdeSolverBody*> &bodies, - int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer); - - void ConvertTypedConstraint( - btTypedConstraint * constraint, - btAlignedObjectArray &joints,int& numJoints, - const btAlignedObjectArray< OdeSolverBody*> &bodies,int _bodyId0,int _bodyId1,btIDebugDraw* debugDrawer); - - -public: - - OdeConstraintSolver(); - - virtual ~OdeConstraintSolver() {} - - virtual btScalar solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc,btDispatcher* dispatcher); - - ///setConstraintForceMixing, the cfm adds some positive value to the main diagonal - ///This can improve convergence (make matrix positive semidefinite), but it can make the simulation look more 'springy' - void setConstraintForceMixing(float cfm) { - m_cfm = cfm; - } - - ///setErrorReductionParamter sets the maximum amount of error reduction - ///which limits energy addition during penetration depth recovery - void setErrorReductionParamter(float erp) - { - m_erp = erp; - } - - ///clear internal cached data and reset random seed - void reset() - { - m_SorLcpSolver.dRand2_seed = 0; - } - - void setRandSeed(unsigned long seed) - { - m_SorLcpSolver.dRand2_seed = seed; - } - unsigned long getRandSeed() const - { - return m_SorLcpSolver.dRand2_seed; - } -}; - - - - -#endif //ODE_CONSTRAINT_SOLVER_H diff --git a/Extras/quickstep/OdeContactJoint.cpp b/Extras/quickstep/OdeContactJoint.cpp deleted file mode 100644 index 89977cdaa..000000000 --- a/Extras/quickstep/OdeContactJoint.cpp +++ /dev/null @@ -1,277 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -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. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -#include "OdeContactJoint.h" -#include "OdeSolverBody.h" -#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" - - -//this constant needs to be set up so different solvers give 'similar' results -#define FRICTION_CONSTANT 120.f - - -ContactJoint::ContactJoint(btPersistentManifold* manifold,int index,bool swap,OdeSolverBody* body0,OdeSolverBody* body1) -:m_manifold(manifold), -m_index(index), -m_swapBodies(swap), -m_body0(body0), -m_body1(body1) -{ -} - -int m_numRows = 3; - - -void ContactJoint::GetInfo1(Info1 *info) -{ - info->m = m_numRows; - //friction adds another 2... - - info->nub = 0; -} - -#define dCROSS(a,op,b,c) \ - (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \ - (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \ - (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); - -#define M_SQRT12 btScalar(0.7071067811865475244008443621048490) - -#define dRecipSqrt(x) ((float)(1.0f/btSqrt(float(x)))) /* reciprocal square root */ - - - -void dPlaneSpace1 (const dVector3 n, dVector3 p, dVector3 q) -{ - if (btFabs(n[2]) > M_SQRT12) { - // choose p in y-z plane - btScalar a = n[1]*n[1] + n[2]*n[2]; - btScalar k = dRecipSqrt (a); - p[0] = 0; - p[1] = -n[2]*k; - p[2] = n[1]*k; - // set q = n x p - q[0] = a*k; - q[1] = -n[0]*p[2]; - q[2] = n[0]*p[1]; - } - else { - // choose p in x-y plane - btScalar a = n[0]*n[0] + n[1]*n[1]; - btScalar k = dRecipSqrt (a); - p[0] = -n[1]*k; - p[1] = n[0]*k; - p[2] = 0; - // set q = n x p - q[0] = -n[2]*p[1]; - q[1] = n[2]*p[0]; - q[2] = a*k; - } -} - - - -void ContactJoint::GetInfo2(Info2 *info) -{ - - int s = info->rowskip; - int s2 = 2*s; - - float swapFactor = m_swapBodies ? -1.f : 1.f; - - // get normal, with sign adjusted for body1/body2 polarity - dVector3 normal; - - - btManifoldPoint& point = m_manifold->getContactPoint(m_index); - - normal[0] = swapFactor*point.m_normalWorldOnB.x(); - normal[1] = swapFactor*point.m_normalWorldOnB.y(); - normal[2] = swapFactor*point.m_normalWorldOnB.z(); - normal[3] = 0; // @@@ hmmm - - assert(m_body0); - // if (GetBody0()) - btVector3 relativePositionA; - { - relativePositionA = point.getPositionWorldOnA() - m_body0->m_centerOfMassPosition; - dVector3 c1; - c1[0] = relativePositionA.x(); - c1[1] = relativePositionA.y(); - c1[2] = relativePositionA.z(); - - // set jacobian for normal - info->J1l[0] = normal[0]; - info->J1l[1] = normal[1]; - info->J1l[2] = normal[2]; - dCROSS (info->J1a,=,c1,normal); - - } - - btVector3 relativePositionB(0,0,0); - if (m_body1) - { - // if (GetBody1()) - - { - dVector3 c2; - btVector3 posBody1 = m_body1 ? m_body1->m_centerOfMassPosition : btVector3(0,0,0); - relativePositionB = point.getPositionWorldOnB() - posBody1; - - // for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] - - // j->node[1].body->pos[i]; - c2[0] = relativePositionB.x(); - c2[1] = relativePositionB.y(); - c2[2] = relativePositionB.z(); - - info->J2l[0] = -normal[0]; - info->J2l[1] = -normal[1]; - info->J2l[2] = -normal[2]; - dCROSS (info->J2a,= -,c2,normal); - } - } - - btScalar k = info->fps * info->erp; - - float depth = -point.getDistance(); -// if (depth < 0.f) -// depth = 0.f; - - info->c[0] = k * depth; - //float maxvel = .2f; - -// if (info->c[0] > maxvel) -// info->c[0] = maxvel; - - - //can override it, not necessary -// info->cfm[0] = 0.f; -// info->cfm[1] = 0.f; -// info->cfm[2] = 0.f; - - - - // set LCP limits for normal - info->lo[0] = 0; - info->hi[0] = 1e30f;//dInfinity; - info->lo[1] = 0; - info->hi[1] = 0.f; - info->lo[2] = 0.f; - info->hi[2] = 0.f; - -#define DO_THE_FRICTION_2 -#ifdef DO_THE_FRICTION_2 - // now do jacobian for tangential forces - dVector3 t1,t2; // two vectors tangential to normal - - dVector3 c1; - c1[0] = relativePositionA.x(); - c1[1] = relativePositionA.y(); - c1[2] = relativePositionA.z(); - - dVector3 c2; - c2[0] = relativePositionB.x(); - c2[1] = relativePositionB.y(); - c2[2] = relativePositionB.z(); - - //combined friction is available in the contact point - float friction = 0.25;//FRICTION_CONSTANT ;//* m_body0->m_friction * m_body1->m_friction; - - // first friction direction - if (m_numRows >= 2) - { - - - - dPlaneSpace1 (normal,t1,t2); - - info->J1l[s+0] = t1[0]; - info->J1l[s+1] = t1[1]; - info->J1l[s+2] = t1[2]; - dCROSS (info->J1a+s,=,c1,t1); - if (1) { //j->node[1].body) { - info->J2l[s+0] = -t1[0]; - info->J2l[s+1] = -t1[1]; - info->J2l[s+2] = -t1[2]; - dCROSS (info->J2a+s,= -,c2,t1); - } - // set right hand side - if (0) {//j->contact.surface.mode & dContactMotion1) { - //info->c[1] = j->contact.surface.motion1; - } - // set LCP bounds and friction index. this depends on the approximation - // mode - //1e30f - - - info->lo[1] = -friction;//-j->contact.surface.mu; - info->hi[1] = friction;//j->contact.surface.mu; - if (1)//j->contact.surface.mode & dContactApprox1_1) - info->findex[1] = 0; - - // set slip (constraint force mixing) - if (0)//j->contact.surface.mode & dContactSlip1) - { - // info->cfm[1] = j->contact.surface.slip1; - } else - { - //info->cfm[1] = 0.f; - } - } - - // second friction direction - if (m_numRows >= 3) { - info->J1l[s2+0] = t2[0]; - info->J1l[s2+1] = t2[1]; - info->J1l[s2+2] = t2[2]; - dCROSS (info->J1a+s2,=,c1,t2); - if (1) { //j->node[1].body) { - info->J2l[s2+0] = -t2[0]; - info->J2l[s2+1] = -t2[1]; - info->J2l[s2+2] = -t2[2]; - dCROSS (info->J2a+s2,= -,c2,t2); - } - - // set right hand side - if (0){//j->contact.surface.mode & dContactMotion2) { - //info->c[2] = j->contact.surface.motion2; - } - // set LCP bounds and friction index. this depends on the approximation - // mode - if (0){//j->contact.surface.mode & dContactMu2) { - //info->lo[2] = -j->contact.surface.mu2; - //info->hi[2] = j->contact.surface.mu2; - } - else { - info->lo[2] = -friction; - info->hi[2] = friction; - } - if (0)//j->contact.surface.mode & dContactApprox1_2) - - { - info->findex[2] = 0; - } - // set slip (constraint force mixing) - if (0) //j->contact.surface.mode & dContactSlip2) - - { - //info->cfm[2] = j->contact.surface.slip2; - - } - } - -#endif //DO_THE_FRICTION_2 - -} - diff --git a/Extras/quickstep/OdeContactJoint.h b/Extras/quickstep/OdeContactJoint.h deleted file mode 100644 index d1d1ef9dd..000000000 --- a/Extras/quickstep/OdeContactJoint.h +++ /dev/null @@ -1,50 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -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. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef CONTACT_JOINT_H -#define CONTACT_JOINT_H - -#include "OdeJoint.h" -struct OdeSolverBody; -class btPersistentManifold; - -class ContactJoint : public BU_Joint -{ - btPersistentManifold* m_manifold; - int m_index; - bool m_swapBodies; - OdeSolverBody* m_body0; - OdeSolverBody* m_body1; - - -public: - - ContactJoint() {}; - - ContactJoint(btPersistentManifold* manifold,int index,bool swap,OdeSolverBody* body0,OdeSolverBody* body1); - - //BU_Joint interface for solver - - virtual void GetInfo1(Info1 *info); - - virtual void GetInfo2(Info2 *info); - - - - -}; - -#endif //CONTACT_JOINT_H - diff --git a/Extras/quickstep/OdeJoint.cpp b/Extras/quickstep/OdeJoint.cpp deleted file mode 100644 index 60d677f11..000000000 --- a/Extras/quickstep/OdeJoint.cpp +++ /dev/null @@ -1,25 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -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. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#include "OdeJoint.h" - -BU_Joint::BU_Joint() -{ - -} -BU_Joint::~BU_Joint() -{ - -} diff --git a/Extras/quickstep/OdeJoint.h b/Extras/quickstep/OdeJoint.h deleted file mode 100644 index 1b8cfb463..000000000 --- a/Extras/quickstep/OdeJoint.h +++ /dev/null @@ -1,94 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -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. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ - -#ifndef BU_Joint_H -#define BU_Joint_H - -struct OdeSolverBody; -class BU_Joint; - -#include "LinearMath/btScalar.h" - -struct BU_ContactJointNode { - BU_Joint *joint; // pointer to enclosing BU_Joint object - OdeSolverBody* body; // *other* body this joint is connected to -}; -typedef btScalar dVector3[4]; - - -class BU_Joint { - -public: - // naming convention: the "first" body this is connected to is node[0].body, - // and the "second" body is node[1].body. if this joint is only connected - // to one body then the second body is 0. - - // info returned by getInfo1 function. the constraint dimension is m (<=6). - // i.e. that is the total number of rows in the jacobian. `nub' is the - // number of unbounded variables (which have lo,hi = -/+ infinity). - - BU_Joint(); - virtual ~BU_Joint(); - - - struct Info1 { - int m,nub; - }; - - // info returned by getInfo2 function - - struct Info2 { - // integrator parameters: frames per second (1/stepsize), default error - // reduction parameter (0..1). - btScalar fps,erp; - - // for the first and second body, pointers to two (linear and angular) - // n*3 jacobian sub matrices, stored by rows. these matrices will have - // been initialized to 0 on entry. if the second body is zero then the - // J2xx pointers may be 0. - btScalar *J1l,*J1a,*J2l,*J2a; - - // elements to jump from one row to the next in J's - int rowskip; - - // right hand sides of the equation J*v = c + cfm * lambda. cfm is the - // "constraint force mixing" vector. c is set to zero on entry, cfm is - // set to a constant value (typically very small or zero) value on entry. - btScalar *c,*cfm; - - // lo and hi limits for variables (set to -/+ infinity on entry). - btScalar *lo,*hi; - - // findex vector for variables. see the LCP solver interface for a - // description of what this does. this is set to -1 on entry. - // note that the returned indexes are relative to the first index of - // the constraint. - int *findex; - }; - - // virtual function table: size of the joint structure, function pointers. - // we do it this way instead of using C++ virtual functions because - // sometimes we need to allocate joints ourself within a memory pool. - - virtual void GetInfo1 (Info1 *info)=0; - virtual void GetInfo2 (Info2 *info)=0; - - int flags; // dJOINT_xxx flags - BU_ContactJointNode node[2]; // connections to bodies. node[1].body can be 0 - btScalar lambda[6]; // lambda generated by last step -}; - - -#endif //BU_Joint_H diff --git a/Extras/quickstep/OdeMacros.h b/Extras/quickstep/OdeMacros.h deleted file mode 100644 index 51eabec72..000000000 --- a/Extras/quickstep/OdeMacros.h +++ /dev/null @@ -1,216 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser bteral Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * bteral Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#define ODE_MACROS -#ifdef ODE_MACROS - -#include "LinearMath/btScalar.h" - -typedef btScalar dVector4[4]; -typedef btScalar dMatrix3[4*3]; -#define dInfinity FLT_MAX - - - -#define dRecip(x) ((float)(1.0f/(x))) /* reciprocal */ - - - -#define dMULTIPLY0_331NEW(A,op,B,C) \ -{\ - btScalar tmp[3];\ - tmp[0] = C.getX();\ - tmp[1] = C.getY();\ - tmp[2] = C.getZ();\ - dMULTIPLYOP0_331(A,op,B,tmp);\ -} - -#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C) -#define dMULTIPLYOP0_331(A,op,B,C) \ - (A)[0] op dDOT1((B),(C)); \ - (A)[1] op dDOT1((B+4),(C)); \ - (A)[2] op dDOT1((B+8),(C)); - -#define dAASSERT btAssert -#define dIASSERT btAssert - -#define REAL float -#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)]) -inline btScalar dDOT1 (const btScalar *a, const btScalar *b) -{ return dDOTpq(a,b,1,1); } -#define dDOT14(a,b) dDOTpq(a,b,1,4) - -#define dCROSS(a,op,b,c) \ - (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \ - (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \ - (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); - -/* - * set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b. - * A is stored by rows, and has `skip' elements per row. the matrix is - * assumed to be already zero, so this does not write zero elements! - * if (plus,minus) is (+,-) then a positive version will be written. - * if (plus,minus) is (-,+) then a negative version will be written. - */ - -#define dCROSSMAT(A,a,skip,plus,minus) \ -do { \ - (A)[1] = minus (a)[2]; \ - (A)[2] = plus (a)[1]; \ - (A)[(skip)+0] = plus (a)[2]; \ - (A)[(skip)+2] = minus (a)[0]; \ - (A)[2*(skip)+0] = minus (a)[1]; \ - (A)[2*(skip)+1] = plus (a)[0]; \ -} while(0) - - -#define dMULTIPLYOP2_333(A,op,B,C) \ - (A)[0] op dDOT1((B),(C)); \ - (A)[1] op dDOT1((B),(C+4)); \ - (A)[2] op dDOT1((B),(C+8)); \ - (A)[4] op dDOT1((B+4),(C)); \ - (A)[5] op dDOT1((B+4),(C+4)); \ - (A)[6] op dDOT1((B+4),(C+8)); \ - (A)[8] op dDOT1((B+8),(C)); \ - (A)[9] op dDOT1((B+8),(C+4)); \ - (A)[10] op dDOT1((B+8),(C+8)); - -#define dMULTIPLYOP0_333(A,op,B,C) \ - (A)[0] op dDOT14((B),(C)); \ - (A)[1] op dDOT14((B),(C+1)); \ - (A)[2] op dDOT14((B),(C+2)); \ - (A)[4] op dDOT14((B+4),(C)); \ - (A)[5] op dDOT14((B+4),(C+1)); \ - (A)[6] op dDOT14((B+4),(C+2)); \ - (A)[8] op dDOT14((B+8),(C)); \ - (A)[9] op dDOT14((B+8),(C+1)); \ - (A)[10] op dDOT14((B+8),(C+2)); - -#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C) -#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C) -#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C) - - -//////////////////////////////////////////////////////////////////// -#define EFFICIENT_ALIGNMENT 16 -#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1) -/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste - * up to 15 bytes per allocation, depending on what alloca() returns. - */ - -#define dALLOCA16(n) \ - ((char*)dEFFICIENT_SIZE(((size_t)(alloca((n)+(EFFICIENT_ALIGNMENT-1)))))) - -//#define ALLOCA dALLOCA16 - -typedef const btScalar *dRealPtr; -typedef btScalar *dRealMutablePtr; -//#define dRealArray(name,n) btScalar name[n]; -//#define dRealAllocaArray(name,n) btScalar *name = (btScalar*) ALLOCA ((n)*sizeof(btScalar)); - -/////////////////////////////////////////////////////////////////////////////// - - //Remotion: 10.10.2007 -#define ALLOCA(size) stackAlloc->allocate( dEFFICIENT_SIZE(size) ); - -//#define dRealAllocaArray(name,size) btScalar *name = (btScalar*) stackAlloc->allocate(dEFFICIENT_SIZE(size)*sizeof(btScalar)); -#define dRealAllocaArray(name,size) btScalar *name = NULL; \ - int memNeeded_##name = dEFFICIENT_SIZE(size)*sizeof(btScalar); \ - if (memNeeded_##name < stackAlloc->getAvailableMemory()) name = (btScalar*) stackAlloc->allocate(memNeeded_##name); \ - else{ btAssert(memNeeded_##name < stackAlloc->getAvailableMemory()); name = (btScalar*) alloca(memNeeded_##name); } - - - - - -/////////////////////////////////////////////////////////////////////////////// -#if 0 -inline void dSetZero1 (btScalar *a, int n) -{ - dAASSERT (a && n >= 0); - while (n > 0) { - *(a++) = 0; - n--; - } -} - -inline void dSetValue1 (btScalar *a, int n, btScalar value) -{ - dAASSERT (a && n >= 0); - while (n > 0) { - *(a++) = value; - n--; - } -} -#else - -/// This macros are for MSVC and XCode compilers. Remotion. - - -#include //for memset - -//Remotion: 10.10.2007 -//------------------------------------------------------------------------------ -#define IS_ALIGNED_16(x) ((size_t(x)&15)==0) -//------------------------------------------------------------------------------ -inline void dSetZero1 (btScalar *dest, int size) -{ - dAASSERT (dest && size >= 0); - memset(dest, 0, size * sizeof(btScalar)); -} -//------------------------------------------------------------------------------ -inline void dSetValue1 (btScalar *dest, int size, btScalar val) -{ - dAASSERT (dest && size >= 0); - int n_mod4 = size & 3; - int n4 = size - n_mod4; -/*#ifdef __USE_SSE__ -//it is not supported on double precision, todo... - if(IS_ALIGNED_16(dest)){ - __m128 xmm0 = _mm_set_ps1(val); - for (int i=0; i - -void OdeTypedJoint::GetInfo1(Info1 *info) -{ - int joint_type = m_constraint->getConstraintType(); - switch (joint_type) - { - case POINT2POINT_CONSTRAINT_TYPE: - { - OdeP2PJoint p2pjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); - p2pjoint.GetInfo1(info); - } - break; - case D6_CONSTRAINT_TYPE: - { - OdeD6Joint d6joint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); - d6joint.GetInfo1(info); - } - break; - }; -} - -void OdeTypedJoint::GetInfo2(Info2 *info) -{ - int joint_type = m_constraint->getConstraintType(); - switch (joint_type) - { - case POINT2POINT_CONSTRAINT_TYPE: - { - OdeP2PJoint p2pjoint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); - p2pjoint.GetInfo2(info); - } - break; - case D6_CONSTRAINT_TYPE: - { - OdeD6Joint d6joint(m_constraint,m_index,m_swapBodies,m_body0,m_body1); - d6joint.GetInfo2(info); - } - break; - }; -} - - -OdeP2PJoint::OdeP2PJoint( - btTypedConstraint * constraint, - int index,bool swap,OdeSolverBody* body0,OdeSolverBody* body1): - OdeTypedJoint(constraint,index,swap,body0,body1) -{ -} - - -void OdeP2PJoint::GetInfo1(Info1 *info) -{ - info->m = 3; - info->nub = 3; -} - - -void OdeP2PJoint::GetInfo2(Info2 *info) -{ - - btPoint2PointConstraint * p2pconstraint = this->getP2PConstraint(); - - //retrieve matrices - btTransform body0_trans; - if (m_body0) - { - body0_trans = m_body0->m_originalBody->getCenterOfMassTransform(); - } -// btScalar body0_mat[12]; -// body0_mat[0] = body0_trans.getBasis()[0][0]; -// body0_mat[1] = body0_trans.getBasis()[0][1]; -// body0_mat[2] = body0_trans.getBasis()[0][2]; -// body0_mat[4] = body0_trans.getBasis()[1][0]; -// body0_mat[5] = body0_trans.getBasis()[1][1]; -// body0_mat[6] = body0_trans.getBasis()[1][2]; -// body0_mat[8] = body0_trans.getBasis()[2][0]; -// body0_mat[9] = body0_trans.getBasis()[2][1]; -// body0_mat[10] = body0_trans.getBasis()[2][2]; - - btTransform body1_trans; - - if (m_body1) - { - body1_trans = m_body1->m_originalBody->getCenterOfMassTransform(); - } -// btScalar body1_mat[12]; -// body1_mat[0] = body1_trans.getBasis()[0][0]; -// body1_mat[1] = body1_trans.getBasis()[0][1]; -// body1_mat[2] = body1_trans.getBasis()[0][2]; -// body1_mat[4] = body1_trans.getBasis()[1][0]; -// body1_mat[5] = body1_trans.getBasis()[1][1]; -// body1_mat[6] = body1_trans.getBasis()[1][2]; -// body1_mat[8] = body1_trans.getBasis()[2][0]; -// body1_mat[9] = body1_trans.getBasis()[2][1]; -// body1_mat[10] = body1_trans.getBasis()[2][2]; - - - - - // anchor points in global coordinates with respect to body PORs. - - - int s = info->rowskip; - - // set jacobian - info->J1l[0] = 1; - info->J1l[s+1] = 1; - info->J1l[2*s+2] = 1; - - - btVector3 a1,a2; - - a1 = body0_trans.getBasis()*p2pconstraint->getPivotInA(); - //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA); - dCROSSMAT (info->J1a,a1,s,-,+); - if (m_body1) - { - info->J2l[0] = -1; - info->J2l[s+1] = -1; - info->J2l[2*s+2] = -1; - a2 = body1_trans.getBasis()*p2pconstraint->getPivotInB(); - //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB); - dCROSSMAT (info->J2a,a2,s,+,-); - } - - - // set right hand side - btScalar k = info->fps * info->erp; - if (m_body1) - { - for (int j=0; j<3; j++) - { - info->c[j] = k * (a2[j] + body1_trans.getOrigin()[j] - - a1[j] - body0_trans.getOrigin()[j]); - } - } - else - { - for (int j=0; j<3; j++) - { - info->c[j] = k * (p2pconstraint->getPivotInB()[j] - a1[j] - - body0_trans.getOrigin()[j]); - } - } -} - - -///////////////////limit motor support - -/*! \pre testLimitValue must be called on limot*/ -int bt_get_limit_motor_info2( - btRotationalLimitMotor * limot, - btRigidBody * body0, btRigidBody * body1, - BU_Joint::Info2 *info, int row, btVector3& ax1, int rotational) -{ - - - int srow = row * info->rowskip; - - // if the joint is powered, or has joint limits, add in the extra row - int powered = limot->m_enableMotor; - int limit = limot->m_currentLimit; - - if (powered || limit) - { - btScalar *J1 = rotational ? info->J1a : info->J1l; - btScalar *J2 = rotational ? info->J2a : info->J2l; - - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - if (body1) - { - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - } - - // linear limot torque decoupling step: - // - // if this is a linear limot (e.g. from a slider), we have to be careful - // that the linear constraint forces (+/- ax1) applied to the two bodies - // do not create a torque couple. in other words, the points that the - // constraint force is applied at must lie along the same ax1 axis. - // a torque couple will result in powered or limited slider-jointed free - // bodies from gaining angular momentum. - // the solution used here is to apply the constraint forces at the point - // halfway between the body centers. there is no penalty (other than an - // extra tiny bit of computation) in doing this adjustment. note that we - // only need to do this if the constraint connects two bodies. - - btVector3 ltd; // Linear Torque Decoupling vector (a torque) - if (!rotational && body1) - { - btVector3 c; - c[0]=btScalar(0.5)*(body1->getCenterOfMassPosition()[0] - -body0->getCenterOfMassPosition()[0]); - c[1]=btScalar(0.5)*(body1->getCenterOfMassPosition()[1] - -body0->getCenterOfMassPosition()[1]); - c[2]=btScalar(0.5)*(body1->getCenterOfMassPosition()[2] - -body0->getCenterOfMassPosition()[2]); - - ltd = c.cross(ax1); - - info->J1a[srow+0] = ltd[0]; - info->J1a[srow+1] = ltd[1]; - info->J1a[srow+2] = ltd[2]; - info->J2a[srow+0] = ltd[0]; - info->J2a[srow+1] = ltd[1]; - info->J2a[srow+2] = ltd[2]; - } - - // if we're limited low and high simultaneously, the joint motor is - // ineffective - - if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0; - - if (powered) - { - info->cfm[row] = 0.0f;//limot->m_normalCFM; - if (! limit) - { - info->c[row] = limot->m_targetVelocity; - info->lo[row] = -limot->m_maxMotorForce; - info->hi[row] = limot->m_maxMotorForce; - } - } - - if (limit) - { - btScalar k = info->fps * limot->m_ERP; - info->c[row] = -k * limot->m_currentLimitError; - info->cfm[row] = 0.0f;//limot->m_stopCFM; - - if (limot->m_loLimit == limot->m_hiLimit) - { - // limited low and high simultaneously - info->lo[row] = -dInfinity; - info->hi[row] = dInfinity; - } - else - { - if (limit == 1) - { - // low limit - info->lo[row] = 0; - info->hi[row] = SIMD_INFINITY; - } - else - { - // high limit - info->lo[row] = -SIMD_INFINITY; - info->hi[row] = 0; - } - - // deal with bounce - if (limot->m_bounce > 0) - { - // calculate joint velocity - btScalar vel; - if (rotational) - { - vel = body0->getAngularVelocity().dot(ax1); - if (body1) - vel -= body1->getAngularVelocity().dot(ax1); - } - else - { - vel = body0->getLinearVelocity().dot(ax1); - if (body1) - vel -= body1->getLinearVelocity().dot(ax1); - } - - // only apply bounce if the velocity is incoming, and if the - // resulting c[] exceeds what we already have. - if (limit == 1) - { - // low limit - if (vel < 0) - { - btScalar newc = -limot->m_bounce* vel; - if (newc > info->c[row]) info->c[row] = newc; - } - } - else - { - // high limit - all those computations are reversed - if (vel > 0) - { - btScalar newc = -limot->m_bounce * vel; - if (newc < info->c[row]) info->c[row] = newc; - } - } - } - } - } - return 1; - } - else return 0; -} - - -///////////////////OdeD6Joint - - - - - -OdeD6Joint::OdeD6Joint( - btTypedConstraint * constraint, - int index,bool swap,OdeSolverBody* body0,OdeSolverBody* body1): - OdeTypedJoint(constraint,index,swap,body0,body1) -{ -} - - -void OdeD6Joint::GetInfo1(Info1 *info) -{ - btGeneric6DofConstraint * d6constraint = this->getD6Constraint(); - //prepare constraint - d6constraint->calculateTransforms(); - info->m = 3; - info->nub = 3; - - //test angular limits - for (int i=0;i<3 ;i++ ) - { - //if(i==2) continue; - if(d6constraint->testAngularLimitMotor(i)) - { - info->m++; - } - } - - -} - - -int OdeD6Joint::setLinearLimits(Info2 *info) -{ - - btGeneric6DofConstraint * d6constraint = this->getD6Constraint(); - - //retrieve matrices - btTransform body0_trans; - if (m_body0) - { - body0_trans = m_body0->m_originalBody->getCenterOfMassTransform(); - } - - btTransform body1_trans; - - if (m_body1) - { - body1_trans = m_body1->m_originalBody->getCenterOfMassTransform(); - } - - // anchor points in global coordinates with respect to body PORs. - - int s = info->rowskip; - - // set jacobian - info->J1l[0] = 1; - info->J1l[s+1] = 1; - info->J1l[2*s+2] = 1; - - - btVector3 a1,a2; - - a1 = body0_trans.getBasis()*d6constraint->getFrameOffsetA().getOrigin(); - //dMULTIPLY0_331 (a1, body0_mat,m_constraint->m_pivotInA); - dCROSSMAT (info->J1a,a1,s,-,+); - if (m_body1) - { - info->J2l[0] = -1; - info->J2l[s+1] = -1; - info->J2l[2*s+2] = -1; - a2 = body1_trans.getBasis()*d6constraint->getFrameOffsetB().getOrigin(); - - //dMULTIPLY0_331 (a2,body1_mat,m_constraint->m_pivotInB); - dCROSSMAT (info->J2a,a2,s,+,-); - } - - - // set right hand side - btScalar k = info->fps * info->erp; - if (m_body1) - { - for (int j=0; j<3; j++) - { - info->c[j] = k * (a2[j] + body1_trans.getOrigin()[j] - - a1[j] - body0_trans.getOrigin()[j]); - } - } - else - { - for (int j=0; j<3; j++) - { - info->c[j] = k * (d6constraint->getCalculatedTransformB().getOrigin()[j] - a1[j] - - body0_trans.getOrigin()[j]); - } - } - - return 3; - -} - -int OdeD6Joint::setAngularLimits(Info2 *info, int row_offset) -{ - btGeneric6DofConstraint * d6constraint = this->getD6Constraint(); - int row = row_offset; - //solve angular limits - for (int i=0;i<3 ;i++ ) - { - //if(i==2) continue; - if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques()) - { - btVector3 axis = d6constraint->getAxis(i); - row += bt_get_limit_motor_info2( - d6constraint->getRotationalLimitMotor(i), - m_body0->m_originalBody,m_body1->m_originalBody,info,row,axis,1); - } - } - - return row; -} - -void OdeD6Joint::GetInfo2(Info2 *info) -{ - int row = setLinearLimits(info); - setAngularLimits(info, row); -} - diff --git a/Extras/quickstep/OdeTypedJoint.h b/Extras/quickstep/OdeTypedJoint.h deleted file mode 100644 index 0888a85ca..000000000 --- a/Extras/quickstep/OdeTypedJoint.h +++ /dev/null @@ -1,111 +0,0 @@ -/* -Bullet Continuous Collision Detection and Physics Library -Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ - -This software is provided 'as-is', without any express or implied warranty. -In no event will the authors be held liable for any damages arising from the use of this software. -Permission is granted to anyone to use this software for any purpose, -including commercial applications, and to alter it and redistribute it freely, -subject to the following restrictions: - -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. -2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. -3. This notice may not be removed or altered from any source distribution. -*/ -/* -2007-09-09 -Added support for typed joints by Francisco León -email: projectileman@yahoo.com -http://gimpact.sf.net -*/ - -#ifndef TYPED_JOINT_H -#define TYPED_JOINT_H - -#include "OdeJoint.h" -#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h" -#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h" - -struct OdeSolverBody; - -class OdeTypedJoint : public BU_Joint -{ -public: - btTypedConstraint * m_constraint; - int m_index; - bool m_swapBodies; - OdeSolverBody* m_body0; - OdeSolverBody* m_body1; - - OdeTypedJoint(){} - OdeTypedJoint( - btTypedConstraint * constraint, - int index,bool swap,OdeSolverBody* body0,OdeSolverBody* body1): - m_constraint(constraint), - m_index(index), - m_swapBodies(swap), - m_body0(body0), - m_body1(body1) - { - } - - virtual void GetInfo1(Info1 *info); - virtual void GetInfo2(Info2 *info); -}; - - - -class OdeP2PJoint : public OdeTypedJoint -{ -protected: - inline btPoint2PointConstraint * getP2PConstraint() - { - return static_cast(m_constraint); - } -public: - - OdeP2PJoint() {}; - - OdeP2PJoint(btTypedConstraint* constraint,int index,bool swap,OdeSolverBody* body0,OdeSolverBody* body1); - - //BU_Joint interface for solver - - virtual void GetInfo1(Info1 *info); - - virtual void GetInfo2(Info2 *info); -}; - - -class OdeD6Joint : public OdeTypedJoint -{ -protected: - inline btGeneric6DofConstraint * getD6Constraint() - { - return static_cast(m_constraint); - } - - int setLinearLimits(Info2 *info); - int setAngularLimits(Info2 *info, int row_offset); - -public: - - OdeD6Joint() {}; - - OdeD6Joint(btTypedConstraint* constraint,int index,bool swap,OdeSolverBody* body0,OdeSolverBody* body1); - - //BU_Joint interface for solver - - virtual void GetInfo1(Info1 *info); - - virtual void GetInfo2(Info2 *info); -}; - -//! retrieves the constraint info from a btRotationalLimitMotor object -/*! \pre testLimitValue must be called on limot*/ -int bt_get_limit_motor_info2( - btRotationalLimitMotor * limot, - btRigidBody * body0, btRigidBody * body1, - BU_Joint::Info2 *info, int row, btVector3& ax1, int rotational); - -#endif //CONTACT_JOINT_H - diff --git a/Extras/quickstep/SorLcp.cpp b/Extras/quickstep/SorLcp.cpp deleted file mode 100644 index 9506dc4b3..000000000 --- a/Extras/quickstep/SorLcp.cpp +++ /dev/null @@ -1,686 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser bteral Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * bteral Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ -#include "SorLcp.h" -#include "OdeSolverBody.h" - -#ifdef USE_SOR_SOLVER - -// SOR LCP taken from ode quickstep, for comparisons to Bullet sequential impulse solver. -#include "LinearMath/btScalar.h" - -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include -#include //FLT_MAX -#ifdef WIN32 -#include -#endif -#include -#include - -#if defined (WIN32) -#include -#else -#if defined (__FreeBSD__) -#include -#else -#include -#endif -#endif - -#include "OdeJoint.h" -#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" -//////////////////////////////////////////////////////////////////// -//math stuff -#include "OdeMacros.h" - -//*************************************************************************** -// configuration - -// for the SOR and CG methods: -// uncomment the following line to use warm starting. this definitely -// help for motor-driven joints. unfortunately it appears to hurt -// with high-friction contacts using the SOR method. use with care - -//#define WARM_STARTING 1 - -// for the SOR method: -// uncomment the following line to randomly reorder constraint rows -// during the solution. depending on the situation, this can help a lot -// or hardly at all, but it doesn't seem to hurt. - -#define RANDOMLY_REORDER_CONSTRAINTS 1 - -//*************************************************************************** -// various common computations involving the matrix J -// compute iMJ = inv(M)*J' -inline void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb, - //OdeSolverBody* const *body, - const btAlignedObjectArray &body, - dRealPtr invI) -{ - int i,j; - dRealMutablePtr iMJ_ptr = iMJ; - dRealMutablePtr J_ptr = J; - for (i=0; im_invMass; - for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j]; - dMULTIPLY0_331 (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3); - if (b2 >= 0) { - k = body[b2]->m_invMass; - for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6]; - dMULTIPLY0_331 (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9); - } - J_ptr += 12; - iMJ_ptr += 12; - } -} - -#if 0 -static void multiply_invM_JTSpecial (int m, int nb, dRealMutablePtr iMJ, int *jb, - dRealMutablePtr in, dRealMutablePtr out,int onlyBody1,int onlyBody2) -{ - int i,j; - - - - dRealMutablePtr out_ptr1 = out + onlyBody1*6; - - for (j=0; j<6; j++) - out_ptr1[j] = 0; - - if (onlyBody2 >= 0) - { - out_ptr1 = out + onlyBody2*6; - - for (j=0; j<6; j++) - out_ptr1[j] = 0; - } - - dRealPtr iMJ_ptr = iMJ; - for (i=0; i= 0) - { - out_ptr = out + b2*6; - for (j=0; j<6; j++) - out_ptr[j] += iMJ_ptr[j] * in[i]; - } - } - - iMJ_ptr += 6; - - } -} -#endif - - -// compute out = inv(M)*J'*in. - -#if 0 -static void multiply_invM_JT (int m, int nb, dRealMutablePtr iMJ, int *jb, - dRealMutablePtr in, dRealMutablePtr out) -{ - int i,j; - dSetZero1 (out,6*nb); - dRealPtr iMJ_ptr = iMJ; - for (i=0; i= 0) { - out_ptr = out + b2*6; - for (j=0; j<6; j++) out_ptr[j] += iMJ_ptr[j] * in[i]; - } - iMJ_ptr += 6; - } -} -#endif - - -// compute out = J*in. -inline void multiply_J (int m, dRealMutablePtr J, int *jb, - dRealMutablePtr in, dRealMutablePtr out) -{ - int i,j; - dRealPtr J_ptr = J; - for (i=0; i= 0) { - in_ptr = in + b2*6; - for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j]; - } - J_ptr += 6; - out[i] = sum; - } -} - -//*************************************************************************** -// SOR-LCP method - -// nb is the number of bodies in the body array. -// J is an m*12 matrix of constraint rows -// jb is an array of first and second body numbers for each constraint row -// invI is the global frame inverse inertia for each body (stacked 3x3 matrices) -// -// this returns lambda and fc (the constraint force). -// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda -// -// b, lo and hi are modified on exit - -//------------------------------------------------------------------------------ -ATTRIBUTE_ALIGNED16(struct) IndexError { - btScalar error; // error to sort on - int findex; - int index; // row index -}; - -//------------------------------------------------------------------------------ -void SorLcpSolver::SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb, - const btAlignedObjectArray &body, - dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs, - dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex, - int numiter,float overRelax, - btStackAlloc* stackAlloc - ) -{ - //btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007 - AutoBlockSa asaBlock(stackAlloc); - - const int num_iterations = numiter; - const float sor_w = overRelax; // SOR over-relaxation parameter - - int i,j; - -#ifdef WARM_STARTING - // for warm starting, this seems to be necessary to prevent - // jerkiness in motor-driven joints. i have no idea why this works. - for (i=0; i= 0) { - for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j]; - } - iMJ_ptr += 12; - J_ptr += 12; - Ad[i] = sor_w / sum;//(sum + cfm[i]); - } - - // scale J and b by Ad - J_ptr = J; - for (i=0; i= 0) - order[j++].index = i; - dIASSERT (j==m); -#endif - - for (int iteration=0; iteration < num_iterations; iteration++) { - -#ifdef REORDER_CONSTRAINTS - // constraints with findex < 0 always come first. - if (iteration < 2) { - // for the first two iterations, solve the constraints in - // the given order - for (i=0; i v2) ? v1 : v2; - if (max > 0) { - //@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max; - order[i].error = dFabs(lambda[i]-last_lambda[i]); - } - else { - order[i].error = dInfinity; - } - order[i].findex = findex[i]; - order[i].index = i; - } - } - qsort (order,m,sizeof(IndexError),&compare_index_error); -#endif -#ifdef RANDOMLY_REORDER_CONSTRAINTS - if ((iteration & 7) == 0) { - for (i=1; i= 0) { - hi[index] = btFabs (hicopy[index] * lambda[findex[index]]); - lo[index] = -hi[index]; - } - - int b1 = jb[index*2]; - int b2 = jb[index*2+1]; - float delta = rhs[index] - lambda[index]*Ad[index]; - dRealMutablePtr fc_ptr = invMforce + 6*b1; - - // @@@ potential optimization: SIMD-ize this and the b2 >= 0 case - delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] + - fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] + - fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5]; - // @@@ potential optimization: handle 1-body constraints in a separate - // loop to avoid the cost of test & jump? - if (b2 >= 0) { - fc_ptr = invMforce + 6*b2; - delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] + - fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] + - fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11]; - } - - // compute lambda and clamp it to [lo,hi]. - // @@@ potential optimization: does SSE have clamping instructions - // to save test+jump penalties here? - float new_lambda = lambda[index] + delta; - if (new_lambda < lo[index]) { - delta = lo[index]-lambda[index]; - lambda[index] = lo[index]; - } - else if (new_lambda > hi[index]) { - delta = hi[index]-lambda[index]; - lambda[index] = hi[index]; - } - else { - lambda[index] = new_lambda; - } - - //@@@ a trick that may or may not help - //float ramp = (1-((float)(iteration+1)/(float)num_iterations)); - //delta *= ramp; - - // update invMforce. - // @@@ potential optimization: SIMD for this and the b2 >= 0 case - fc_ptr = invMforce + 6*b1; - fc_ptr[0] += delta * iMJ_ptr[0]; - fc_ptr[1] += delta * iMJ_ptr[1]; - fc_ptr[2] += delta * iMJ_ptr[2]; - fc_ptr[3] += delta * iMJ_ptr[3]; - fc_ptr[4] += delta * iMJ_ptr[4]; - fc_ptr[5] += delta * iMJ_ptr[5]; - // @@@ potential optimization: handle 1-body constraints in a separate - // loop to avoid the cost of test & jump? - if (b2 >= 0) { - fc_ptr = invMforce + 6*b2; - fc_ptr[0] += delta * iMJ_ptr[6]; - fc_ptr[1] += delta * iMJ_ptr[7]; - fc_ptr[2] += delta * iMJ_ptr[8]; - fc_ptr[3] += delta * iMJ_ptr[9]; - fc_ptr[4] += delta * iMJ_ptr[10]; - fc_ptr[5] += delta * iMJ_ptr[11]; - } - } - } - //stackAlloc->endBlock(saBlock);//Remo: 10.10.2007 -} - -//------------------------------------------------------------------------------ -void SorLcpSolver::SolveInternal1 ( - float global_cfm, - float global_erp, - const btAlignedObjectArray &body, int nb, - btAlignedObjectArray &joint, - int nj, const btContactSolverInfo& solverInfo, - btStackAlloc* stackAlloc) -{ - //btBlock* saBlock = stackAlloc->beginBlock();//Remo: 10.10.2007 - AutoBlockSa asaBlock(stackAlloc); - - int numIter = solverInfo.m_numIterations; - float sOr = solverInfo.m_sor; - - int i,j; - - btScalar stepsize1 = dRecip(solverInfo.m_timeStep); - - // number all bodies in the body list - set their tag values - for (i=0; im_odeTag = i; - - // make a local copy of the joint array, because we might want to modify it. - // (the "BU_Joint *const*" declaration says we're allowed to modify the joints - // but not the joint array, because the caller might need it unchanged). - //@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints - //BU_Joint **joint = (BU_Joint**) alloca (nj * sizeof(BU_Joint*)); - //memcpy (joint,_joint,nj * sizeof(BU_Joint*)); - - // for all bodies, compute the inertia tensor and its inverse in the global - // frame, and compute the rotational force and add it to the torque - // accumulator. I and invI are a vertical stack of 3x4 matrices, one per body. - dRealAllocaArray (I,3*4*nb); - dRealAllocaArray (invI,3*4*nb); -/* for (i=0; im_I,body[i]->m_R); - // compute inverse inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); - dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); - // compute rotational force - dCROSS (body[i]->m_tacc,-=,body[i]->getAngularVelocity(),tmp); - } -*/ - for (i=0; im_I,body[i]->m_R); - dMULTIPLY0_333 (I+i*12,body[i]->m_R,tmp); - - // compute inverse inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->m_invI,body[i]->m_R); - dMULTIPLY0_333 (invI+i*12,body[i]->m_R,tmp); - // compute rotational force -// dMULTIPLY0_331 (tmp,I+i*12,body[i]->m_angularVelocity); -// dCROSS (body[i]->m_tacc,-=,body[i]->m_angularVelocity,tmp); - } - - - - - // get joint information (m = total constraint dimension, nub = number of unbounded variables). - // joints with m=0 are inactive and are removed from the joints array - // entirely, so that the code that follows does not consider them. - //@@@ do we really need to save all the info1's - BU_Joint::Info1 *info = (BU_Joint::Info1*) ALLOCA (nj*sizeof(BU_Joint::Info1)); - - for (i=0, j=0; jGetInfo1 (info+i); - dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); - if (info[i].m > 0) { - joint[i] = joint[j]; - i++; - } - } - nj = i; - - // create the row offset array - int m = 0; - int *ofs = (int*) ALLOCA (nj*sizeof(int)); - for (i=0; i 0) { - // create a constraint equation right hand side vector `c', a constraint - // force mixing vector `cfm', and LCP low and high bound vectors, and an - // 'findex' vector. - dRealAllocaArray (c,m); - dRealAllocaArray (cfm,m); - dRealAllocaArray (lo,m); - dRealAllocaArray (hi,m); - - int *findex = (int*) ALLOCA (m*sizeof(int)); - - dSetZero1 (c,m); - dSetValue1 (cfm,m,global_cfm); - dSetValue1 (lo,m,-dInfinity); - dSetValue1 (hi,m, dInfinity); - for (i=0; iGetInfo2 (&Jinfo); - - if (Jinfo.c[0] > solverInfo.m_maxErrorReduction) - Jinfo.c[0] = solverInfo.m_maxErrorReduction; - - // adjust returned findex values for global index numbering - for (j=0; j= 0) - findex[ofs[i] + j] += ofs[i]; - } - } - - // create an array of body numbers for each joint row - int *jb_ptr = jb; - for (i=0; inode[0].body) ? (joint[i]->node[0].body->m_odeTag) : -1; - int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->m_odeTag) : -1; - for (j=0; jm_invMass; - for (j=0; j<3; j++) - tmp1[i*6+j] = body[i]->m_facc[j] * body_invMass + body[i]->m_linearVelocity[j] * stepsize1; - dMULTIPLY0_331NEW (tmp1 + i*6 + 3,=,invI + i*12,body[i]->m_tacc); - for (j=0; j<3; j++) - tmp1[i*6+3+j] += body[i]->m_angularVelocity[j] * stepsize1; - } - - // put J*tmp1 into rhs - dRealAllocaArray (rhs,m); - multiply_J (m,J,jb,tmp1,rhs); - - // complete rhs - for (i=0; ilambda,info[i].m * sizeof(btScalar)); - } -#endif - - // solve the LCP problem and get lambda and invM*constraint_force - dRealAllocaArray (cforce,nb*6); - - /// SOR_LCP - SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,numIter,sOr,stackAlloc); - -#ifdef WARM_STARTING - // save lambda for the next iteration - //@@@ note that this doesn't work for contact joints yet, as they are - // recreated every iteration - for (i=0; ilambda,lambda+ofs[i],info[i].m * sizeof(btScalar)); - } -#endif - - // note that the SOR method overwrites rhs and J at this point, so - // they should not be used again. - // add stepsize * cforce to the body velocity - for (i=0; im_linearVelocity[j] += solverInfo.m_timeStep* cforce[i*6+j]; - for (j=0; j<3; j++) - body[i]->m_angularVelocity[j] += solverInfo.m_timeStep* cforce[i*6+3+j]; - - } - } - - // compute the velocity update: - // add stepsize * invM * fe to the body velocity - for (i=0; im_invMass; - btVector3 linvel = body[i]->m_linearVelocity; - btVector3 angvel = body[i]->m_angularVelocity; - - for (j=0; j<3; j++) - { - linvel[j] += solverInfo.m_timeStep * body_invMass * body[i]->m_facc[j]; - } - for (j=0; j<3; j++) - { - body[i]->m_tacc[j] *= solverInfo.m_timeStep; - } - dMULTIPLY0_331NEW(angvel,+=,invI + i*12,body[i]->m_tacc); - body[i]->m_angularVelocity = angvel; - } - //stackAlloc->endBlock(saBlock);//Remo: 10.10.2007 -} - - -#endif //USE_SOR_SOLVER diff --git a/Extras/quickstep/SorLcp.h b/Extras/quickstep/SorLcp.h deleted file mode 100644 index f2d8b7618..000000000 --- a/Extras/quickstep/SorLcp.h +++ /dev/null @@ -1,116 +0,0 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * - * All rights reserved. Email: russ@q12.org Web: www.q12.org * - * * - * This library is free software; you can redistribute it and/or * - * modify it under the terms of EITHER: * - * (1) The GNU Lesser bteral Public License as published by the Free * - * Software Foundation; either version 2.1 of the License, or (at * - * your option) any later version. The text of the GNU Lesser * - * bteral Public License is included with this library in the * - * file LICENSE.TXT. * - * (2) The BSD-style license that is included with this library in * - * the file LICENSE-BSD.TXT. * - * * - * This library is distributed in the hope that it will be useful, * - * but WITHOUT ANY WARRANTY; without even the implied warranty of * - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * - * LICENSE.TXT and LICENSE-BSD.TXT for more details. * - * * - *************************************************************************/ - -#define USE_SOR_SOLVER -#ifdef USE_SOR_SOLVER - -#ifndef SOR_LCP_H -#define SOR_LCP_H -struct OdeSolverBody; -class BU_Joint; -#include "LinearMath/btScalar.h" -#include "LinearMath/btAlignedObjectArray.h" -#include "LinearMath/btStackAlloc.h" - -struct btContactSolverInfo; - - -//============================================================================= -class SorLcpSolver //Remotion: 11.10.2007 -{ -public: - SorLcpSolver() - { - dRand2_seed = 0; - } - - void SolveInternal1 (float global_cfm, - float global_erp, - const btAlignedObjectArray &body, int nb, - btAlignedObjectArray &joint, - int nj, const btContactSolverInfo& solverInfo, - btStackAlloc* stackAlloc - ); - -public: //data - unsigned long dRand2_seed; - -protected: //typedef - typedef const btScalar *dRealPtr; - typedef btScalar *dRealMutablePtr; - -protected: //members - //------------------------------------------------------------------------------ - SIMD_FORCE_INLINE unsigned long dRand2() - { - dRand2_seed = (1664525L*dRand2_seed + 1013904223L) & 0xffffffff; - return dRand2_seed; - } - //------------------------------------------------------------------------------ - SIMD_FORCE_INLINE int dRandInt2 (int n) - { - float a = float(n) / 4294967296.0f; - return (int) (float(dRand2()) * a); - } - //------------------------------------------------------------------------------ - void SOR_LCP(int m, int nb, dRealMutablePtr J, int *jb, - const btAlignedObjectArray &body, - dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr invMforce, dRealMutablePtr rhs, - dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex, - int numiter,float overRelax, - btStackAlloc* stackAlloc - ); -}; - - -//============================================================================= -class AutoBlockSa //Remotion: 10.10.2007 -{ - btStackAlloc* stackAlloc; - btBlock* saBlock; -public: - AutoBlockSa(btStackAlloc* stackAlloc_) - { - stackAlloc = stackAlloc_; - saBlock = stackAlloc->beginBlock(); - } - ~AutoBlockSa() - { - stackAlloc->endBlock(saBlock); - } - //operator btBlock* () { return saBlock; } -}; -// //Usage -//void function(btStackAlloc* stackAlloc) -//{ -// AutoBlockSa(stackAlloc); -// ... -// if(...) return; -// return; -//} -//------------------------------------------------------------------------------ - - -#endif //SOR_LCP_H - -#endif //USE_SOR_SOLVER -