From 643b0be4cb3f73bd876493d2a7bd6f76ef27cf06 Mon Sep 17 00:00:00 2001 From: Sergej Reich Date: Thu, 7 Mar 2013 17:53:16 +0000 Subject: bullet: Update to current svn, r2636 Apply patches in patches directory, remove patches that were applied upstream. If you made changes without adding a patch, please check. Fixes [#32233] exporting bullet format results in corrupt files. --- .../ConstraintSolver/btConeTwistConstraint.cpp | 43 +- .../ConstraintSolver/btConeTwistConstraint.h | 7 +- .../ConstraintSolver/btContactConstraint.cpp | 4 +- .../ConstraintSolver/btContactSolverInfo.h | 86 +- .../ConstraintSolver/btGearConstraint.cpp | 54 ++ .../ConstraintSolver/btGearConstraint.h | 56 ++ .../ConstraintSolver/btGeneric6DofConstraint.cpp | 47 +- .../ConstraintSolver/btGeneric6DofConstraint.h | 6 +- .../btGeneric6DofSpringConstraint.cpp | 2 +- .../btGeneric6DofSpringConstraint.h | 13 +- .../ConstraintSolver/btHinge2Constraint.h | 4 +- .../ConstraintSolver/btHingeConstraint.cpp | 18 +- .../ConstraintSolver/btHingeConstraint.h | 2 + .../ConstraintSolver/btJacobianEntry.h | 3 +- .../ConstraintSolver/btPoint2PointConstraint.cpp | 9 +- .../ConstraintSolver/btPoint2PointConstraint.h | 2 + .../btSequentialImpulseConstraintSolver.cpp | 1016 ++++++++++++++------ .../btSequentialImpulseConstraintSolver.h | 65 +- .../ConstraintSolver/btSliderConstraint.cpp | 7 + .../ConstraintSolver/btSliderConstraint.h | 4 +- .../BulletDynamics/ConstraintSolver/btSolverBody.h | 134 ++- .../ConstraintSolver/btSolverConstraint.h | 51 +- .../ConstraintSolver/btTypedConstraint.cpp | 6 +- .../ConstraintSolver/btTypedConstraint.h | 36 +- .../ConstraintSolver/btUniversalConstraint.h | 5 +- 25 files changed, 1202 insertions(+), 478 deletions(-) create mode 100644 extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp create mode 100644 extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h mode change 100644 => 100755 extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp mode change 100644 => 100755 extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h (limited to 'extern/bullet2/src/BulletDynamics/ConstraintSolver') diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp index 755544f0dee..15a4c92de20 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.cpp @@ -53,6 +53,7 @@ btConeTwistConstraint::btConeTwistConstraint(btRigidBody& rbA,const btTransform& m_useSolveConstraintObsolete(CONETWIST_USE_OBSOLETE_SOLVER) { m_rbBFrame = m_rbAFrame; + m_rbBFrame.setOrigin(btVector3(0., 0., 0.)); init(); } @@ -136,6 +137,9 @@ void btConeTwistConstraint::getInfo2NonVirtual (btConstraintInfo2* info,const bt btVector3 a1neg = -a1; a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); } + info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[info->rowskip+1] = -1; + info->m_J2linearAxis[2*info->rowskip+2] = -1; btVector3 a2 = transB.getBasis() * m_rbBFrame.getOrigin(); { btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); @@ -304,7 +308,7 @@ void btConeTwistConstraint::buildJacobian() -void btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep) +void btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep) { #ifndef __SPU__ if (m_useSolveConstraintObsolete) @@ -506,7 +510,7 @@ void btConeTwistConstraint::solveConstraintObsolete(btRigidBody& bodyA,btRigidBo m_accTwistLimitImpulse = btMax(m_accTwistLimitImpulse + impulseMag, btScalar(0.0) ); impulseMag = m_accTwistLimitImpulse - temp; - btVector3 impulse = m_twistAxis * impulseMag; + // btVector3 impulse = m_twistAxis * impulseMag; bodyA.internalApplyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*m_twistAxis,impulseMag); bodyB.internalApplyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*m_twistAxis,-impulseMag); @@ -725,7 +729,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr { if(m_swingSpan1 < m_fixThresh) { // hinge around Y axis - if(!(btFuzzyZero(y))) +// if(!(btFuzzyZero(y))) + if((!(btFuzzyZero(x))) || (!(btFuzzyZero(z)))) { m_solveSwingLimit = true; if(m_swingSpan2 >= m_fixThresh) @@ -747,7 +752,8 @@ void btConeTwistConstraint::calcAngleInfo2(const btTransform& transA, const btTr } else { // hinge around Z axis - if(!btFuzzyZero(z)) +// if(!btFuzzyZero(z)) + if((!(btFuzzyZero(x))) || (!(btFuzzyZero(y)))) { m_solveSwingLimit = true; if(m_swingSpan1 >= m_fixThresh) @@ -828,12 +834,11 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, { vSwingAxis = btVector3(qCone.x(), qCone.y(), qCone.z()); vSwingAxis.normalize(); - if (fabs(vSwingAxis.x()) > SIMD_EPSILON) - { - // non-zero twist?! this should never happen. - int wtf = 0; wtf = wtf; - } - +#if 0 + // non-zero twist?! this should never happen. + btAssert(fabs(vSwingAxis.x()) <= SIMD_EPSILON)); +#endif + // Compute limit for given swing. tricky: // Given a swing axis, we're looking for the intersection with the bounding cone ellipse. // (Since we're dealing with angles, this ellipse is embedded on the surface of a sphere.) @@ -877,8 +882,10 @@ void btConeTwistConstraint::computeConeLimitInfo(const btQuaternion& qCone, else if (swingAngle < 0) { // this should never happen! - int wtf = 0; wtf = wtf; - } +#if 0 + btAssert(0); +#endif + } } btVector3 btConeTwistConstraint::GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const @@ -929,7 +936,9 @@ void btConeTwistConstraint::computeTwistLimitInfo(const btQuaternion& qTwist, if (twistAngle < 0) { // this should never happen - int wtf = 0; wtf = wtf; +#if 0 + btAssert(0); +#endif } vTwistAxis = btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z()); @@ -976,10 +985,10 @@ void btConeTwistConstraint::setMotorTarget(const btQuaternion &q) { btTransform trACur = m_rbA.getCenterOfMassTransform(); btTransform trBCur = m_rbB.getCenterOfMassTransform(); - btTransform trABCur = trBCur.inverse() * trACur; - btQuaternion qABCur = trABCur.getRotation(); - btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); - btQuaternion qConstraintCur = trConstraintCur.getRotation(); +// btTransform trABCur = trBCur.inverse() * trACur; +// btQuaternion qABCur = trABCur.getRotation(); +// btTransform trConstraintCur = (trBCur * m_rbBFrame).inverse() * (trACur * m_rbAFrame); + //btQuaternion qConstraintCur = trConstraintCur.getRotation(); btQuaternion qConstraint = m_rbBFrame.getRotation().inverse() * q * m_rbAFrame.getRotation(); setMotorTargetInConstraintSpace(qConstraint); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h index 868e62f063e..09c048beda8 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btConeTwistConstraint.h @@ -50,7 +50,7 @@ enum btConeTwistFlags }; ///btConeTwistConstraint can be used to simulate ragdoll joints (upper arm, leg etc) -class btConeTwistConstraint : public btTypedConstraint +ATTRIBUTE_ALIGNED16(class) btConeTwistConstraint : public btTypedConstraint { #ifdef IN_PARALLELL_SOLVER public: @@ -126,6 +126,8 @@ protected: public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + btConeTwistConstraint(btRigidBody& rbA,btRigidBody& rbB,const btTransform& rbAFrame, const btTransform& rbBFrame); btConeTwistConstraint(btRigidBody& rbA,const btTransform& rbAFrame); @@ -140,8 +142,9 @@ public: void getInfo2NonVirtual(btConstraintInfo2* info,const btTransform& transA,const btTransform& transB,const btMatrix3x3& invInertiaWorldA,const btMatrix3x3& invInertiaWorldB); - virtual void solveConstraintObsolete(btRigidBody& bodyA,btRigidBody& bodyB,btScalar timeStep); + virtual void solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar timeStep); + void updateRHS(btScalar timeStep); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp index 88859182925..9d60d9957a5 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactConstraint.cpp @@ -70,7 +70,7 @@ void btContactConstraint::buildJacobian() -//response between two dynamic objects without friction, assuming 0 penetration depth +//response between two dynamic objects without friction and no restitution, assuming 0 penetration depth btScalar resolveSingleCollision( btRigidBody* body1, btCollisionObject* colObj2, @@ -93,7 +93,7 @@ btScalar resolveSingleCollision( btScalar rel_vel; rel_vel = normal.dot(vel); - btScalar combinedRestitution = body1->getRestitution() * colObj2->getRestitution(); + btScalar combinedRestitution = 0.f; btScalar restitution = combinedRestitution* -rel_vel; btScalar positionalError = solverInfo.m_erp *-distance /solverInfo.m_timeStep ; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 6204cb3d16c..c07e9bbd806 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -16,18 +16,20 @@ subject to the following restrictions: #ifndef BT_CONTACT_SOLVER_INFO #define BT_CONTACT_SOLVER_INFO +#include "LinearMath/btScalar.h" + enum btSolverMode { SOLVER_RANDMIZE_ORDER = 1, SOLVER_FRICTION_SEPARATE = 2, SOLVER_USE_WARMSTARTING = 4, - SOLVER_USE_FRICTION_WARMSTARTING = 8, SOLVER_USE_2_FRICTION_DIRECTIONS = 16, SOLVER_ENABLE_FRICTION_DIRECTION_CACHING = 32, SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION = 64, SOLVER_CACHE_FRIENDLY = 128, - SOLVER_SIMD = 256, //enabled for Windows, the solver innerloop is branchless SIMD, 40% faster than FPU/scalar version - SOLVER_CUDA = 512 //will be open sourced during Game Developers Conference 2009. Much faster. + SOLVER_SIMD = 256, + SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512, + SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024 }; struct btContactSolverInfoData @@ -47,12 +49,15 @@ struct btContactSolverInfoData btScalar m_globalCfm;//constraint force mixing int m_splitImpulse; btScalar m_splitImpulsePenetrationThreshold; + btScalar m_splitImpulseTurnErp; btScalar m_linearSlop; btScalar m_warmstartingFactor; int m_solverMode; int m_restingContactRestitutionThreshold; int m_minimumSolverBatchSize; + btScalar m_maxGyroscopicForce; + btScalar m_singleAxisRollingFrictionThreshold; }; @@ -67,21 +72,88 @@ struct btContactSolverInfo : public btContactSolverInfoData m_tau = btScalar(0.6); m_damping = btScalar(1.0); m_friction = btScalar(0.3); + m_timeStep = btScalar(1.f/60.f); m_restitution = btScalar(0.); m_maxErrorReduction = btScalar(20.); m_numIterations = 10; m_erp = btScalar(0.2); - m_erp2 = btScalar(0.1); + m_erp2 = btScalar(0.8); m_globalCfm = btScalar(0.); m_sor = btScalar(1.); - m_splitImpulse = false; - m_splitImpulsePenetrationThreshold = -0.02f; + m_splitImpulse = true; + m_splitImpulsePenetrationThreshold = -.04f; + m_splitImpulseTurnErp = 0.1f; m_linearSlop = btScalar(0.0); m_warmstartingFactor=btScalar(0.85); + //m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER; m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER; - m_restingContactRestitutionThreshold = 2;//resting contact lifetime threshold to disable restitution + m_restingContactRestitutionThreshold = 2;//unused as of 2.81 m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit + m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their BT_ENABLE_GYROPSCOPIC_FORCE flag set (using btRigidBody::setFlag) + m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows. } }; +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btContactSolverInfoDoubleData +{ + double m_tau; + double m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'. + double m_friction; + double m_timeStep; + double m_restitution; + double m_maxErrorReduction; + double m_sor; + double m_erp;//used as Baumgarte factor + double m_erp2;//used in Split Impulse + double m_globalCfm;//constraint force mixing + double m_splitImpulsePenetrationThreshold; + double m_splitImpulseTurnErp; + double m_linearSlop; + double m_warmstartingFactor; + double m_maxGyroscopicForce; + double m_singleAxisRollingFrictionThreshold; + + int m_numIterations; + int m_solverMode; + int m_restingContactRestitutionThreshold; + int m_minimumSolverBatchSize; + int m_splitImpulse; + char m_padding[4]; + +}; +///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 +struct btContactSolverInfoFloatData +{ + float m_tau; + float m_damping;//global non-contact constraint damping, can be locally overridden by constraints during 'getInfo2'. + float m_friction; + float m_timeStep; + + float m_restitution; + float m_maxErrorReduction; + float m_sor; + float m_erp;//used as Baumgarte factor + + float m_erp2;//used in Split Impulse + float m_globalCfm;//constraint force mixing + float m_splitImpulsePenetrationThreshold; + float m_splitImpulseTurnErp; + + float m_linearSlop; + float m_warmstartingFactor; + float m_maxGyroscopicForce; + float m_singleAxisRollingFrictionThreshold; + + int m_numIterations; + int m_solverMode; + int m_restingContactRestitutionThreshold; + int m_minimumSolverBatchSize; + + int m_splitImpulse; + char m_padding[4]; +}; + + + #endif //BT_CONTACT_SOLVER_INFO diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp new file mode 100644 index 00000000000..bcd457b6731 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.cpp @@ -0,0 +1,54 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2012 Advanced Micro Devices, Inc. http://bulletphysics.org + +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. +*/ + +/// Implemented by Erwin Coumans. The idea for the constraint comes from Dimitris Papavasiliou. + +#include "btGearConstraint.h" + +btGearConstraint::btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio) +:btTypedConstraint(GEAR_CONSTRAINT_TYPE,rbA,rbB), +m_axisInA(axisInA), +m_axisInB(axisInB), +m_ratio(ratio) +{ +} + +btGearConstraint::~btGearConstraint () +{ +} + +void btGearConstraint::getInfo1 (btConstraintInfo1* info) +{ + info->m_numConstraintRows = 1; + info->nub = 1; +} + +void btGearConstraint::getInfo2 (btConstraintInfo2* info) +{ + btVector3 globalAxisA, globalAxisB; + + globalAxisA = m_rbA.getWorldTransform().getBasis()*this->m_axisInA; + globalAxisB = m_rbB.getWorldTransform().getBasis()*this->m_axisInB; + + info->m_J1angularAxis[0] = globalAxisA[0]; + info->m_J1angularAxis[1] = globalAxisA[1]; + info->m_J1angularAxis[2] = globalAxisA[2]; + + info->m_J2angularAxis[0] = m_ratio*globalAxisB[0]; + info->m_J2angularAxis[1] = m_ratio*globalAxisB[1]; + info->m_J2angularAxis[2] = m_ratio*globalAxisB[2]; + +} + diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h new file mode 100644 index 00000000000..60f60094843 --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGearConstraint.h @@ -0,0 +1,56 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2012 Advanced Micro Devices, Inc. http://bulletphysics.org + +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 BT_GEAR_CONSTRAINT_H +#define BT_GEAR_CONSTRAINT_H + +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +///The btGeatConstraint will couple the angular velocity for two bodies around given local axis and ratio. +///See Bullet/Demos/ConstraintDemo for an example use. +class btGearConstraint : public btTypedConstraint +{ +protected: + btVector3 m_axisInA; + btVector3 m_axisInB; + bool m_useFrameA; + btScalar m_ratio; + +public: + btGearConstraint(btRigidBody& rbA, btRigidBody& rbB, const btVector3& axisInA,const btVector3& axisInB, btScalar ratio=1.f); + virtual ~btGearConstraint (); + + ///internal method used by the constraint solver, don't use them directly + virtual void getInfo1 (btConstraintInfo1* info); + + ///internal method used by the constraint solver, don't use them directly + virtual void getInfo2 (btConstraintInfo2* info); + + virtual void setParam(int num, btScalar value, int axis = -1) + { + btAssert(0); + }; + + ///return the local value of parameter + virtual btScalar getParam(int num, int axis = -1) const + { + btAssert(0); + return 0.f; + } + +}; + +#endif //BT_GEAR_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp index 8ff9940bba3..bc2b5a85df9 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp @@ -174,10 +174,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits( // current velocity difference - btVector3 angVelA; - body0->internalGetAngularVelocity(angVelA); - btVector3 angVelB; - body1->internalGetAngularVelocity(angVelB); + btVector3 angVelA = body0->getAngularVelocity(); + btVector3 angVelB = body1->getAngularVelocity(); btVector3 vel_diff; vel_diff = angVelA-angVelB; @@ -225,12 +223,8 @@ btScalar btRotationalLimitMotor::solveAngularLimits( btVector3 motorImp = clippedMotorImpulse * axis; - //body0->applyTorqueImpulse(motorImp); - //body1->applyTorqueImpulse(-motorImp); - - body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse); - body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse); - + body0->applyTorqueImpulse(motorImp); + body1->applyTorqueImpulse(-motorImp); return clippedMotorImpulse; @@ -292,10 +286,8 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition(); btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition(); - btVector3 vel1; - body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1); - btVector3 vel2; - body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2); + btVector3 vel1 = body1.getVelocityInLocalPoint(rel_pos1); + btVector3 vel2 = body2.getVelocityInLocalPoint(rel_pos2); btVector3 vel = vel1 - vel2; btScalar rel_vel = axis_normal_on_a.dot(vel); @@ -348,16 +340,10 @@ btScalar btTranslationalLimitMotor::solveLinearAxis( normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse; btVector3 impulse_vector = axis_normal_on_a * normalImpulse; - //body1.applyImpulse( impulse_vector, rel_pos1); - //body2.applyImpulse(-impulse_vector, rel_pos2); - - btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a); - btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a); - body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse); - body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse); - - + body1.applyImpulse( impulse_vector, rel_pos1); + body2.applyImpulse(-impulse_vector, rel_pos2); + return normalImpulse; } @@ -795,17 +781,16 @@ int btGeneric6DofConstraint::get_limit_motor_info2( if (powered || limit) { // if the joint is powered, or has joint limits, add in the extra row btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis; - btScalar *J2 = rotational ? info->m_J2angularAxis : 0; + btScalar *J2 = rotational ? info->m_J2angularAxis : info->m_J2linearAxis; J1[srow+0] = ax1[0]; J1[srow+1] = ax1[1]; J1[srow+2] = ax1[2]; - if(rotational) - { - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - } - if((!rotational)) + + J2[srow+0] = -ax1[0]; + J2[srow+1] = -ax1[1]; + J2[srow+2] = -ax1[2]; + + if((!rotational)) { if (m_useOffsetForConstraintFrame) { diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h index a8e7bc22902..0409f95379b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h @@ -268,7 +268,7 @@ This brings support for limit parameters and motors. */ -class btGeneric6DofConstraint : public btTypedConstraint +ATTRIBUTE_ALIGNED16(class) btGeneric6DofConstraint : public btTypedConstraint { protected: @@ -346,6 +346,8 @@ protected: public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + ///for backwards compatibility during the transition to 'getInfo/getInfo2' bool m_useSolveConstraintObsolete; @@ -354,7 +356,7 @@ public: //! Calcs global transform of the offsets /*! - Calcs the global transform for the joint offset for body A an B, and also calcs the angle differences between the bodies. + Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies. \sa btGeneric6DofConstraint.getCalculatedTransformA , btGeneric6DofConstraint.getCalculatedTransformB, btGeneric6DofConstraint.calculateAngleInfo */ void calculateTransforms(const btTransform& transA,const btTransform& transB); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp index 2b38714987b..6f765884ec0 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp @@ -118,7 +118,7 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf { // it is assumed that calculateTransforms() have been called before this call int i; - btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity(); + //btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity(); for(i = 0; i < 3; i++) { if(m_springEnabled[i]) diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h index 31e0cd531ae..6fabb30369b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h @@ -32,7 +32,7 @@ subject to the following restrictions: /// 4 : rotation Y (2nd Euler rotational around new position of Y axis, range [-PI/2+epsilon, PI/2-epsilon] ) /// 5 : rotation Z (1st Euler rotational around Z axis, range [-PI+epsilon, PI-epsilon] ) -class btGeneric6DofSpringConstraint : public btGeneric6DofConstraint +ATTRIBUTE_ALIGNED16(class) btGeneric6DofSpringConstraint : public btGeneric6DofConstraint { protected: bool m_springEnabled[6]; @@ -42,6 +42,9 @@ protected: void init(); void internalUpdateSprings(btConstraintInfo2* info); public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); btGeneric6DofSpringConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB); void enableSpring(int index, bool onOff); @@ -87,12 +90,12 @@ SIMD_FORCE_INLINE const char* btGeneric6DofSpringConstraint::serialize(void* dat int i; for (i=0;i<6;i++) { - dof->m_equilibriumPoint[i] = m_equilibriumPoint[i]; - dof->m_springDamping[i] = m_springDamping[i]; + dof->m_equilibriumPoint[i] = (float)m_equilibriumPoint[i]; + dof->m_springDamping[i] = (float)m_springDamping[i]; dof->m_springEnabled[i] = m_springEnabled[i]? 1 : 0; - dof->m_springStiffness[i] = m_springStiffness[i]; + dof->m_springStiffness[i] = (float)m_springStiffness[i]; } - return "btGeneric6DofConstraintData"; + return "btGeneric6DofSpringConstraintData"; } #endif // BT_GENERIC_6DOF_SPRING_CONSTRAINT_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h index a76452ddb64..9a004986911 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHinge2Constraint.h @@ -29,13 +29,15 @@ subject to the following restrictions: // 2 rotational degrees of freedom, similar to Euler rotations around Z (axis 1) and X (axis 2) // 1 translational (along axis Z) with suspension spring -class btHinge2Constraint : public btGeneric6DofSpringConstraint +ATTRIBUTE_ALIGNED16(class) btHinge2Constraint : public btGeneric6DofSpringConstraint { protected: btVector3 m_anchor; btVector3 m_axis1; btVector3 m_axis2; public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + // constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 9e3a2baeed9..c1897413078 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp @@ -369,6 +369,10 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf info->m_J1angularAxis[i*skip+1]=0; info->m_J1angularAxis[i*skip+2]=0; + info->m_J2linearAxis[i*skip]=0; + info->m_J2linearAxis[i*skip+1]=0; + info->m_J2linearAxis[i*skip+2]=0; + info->m_J2angularAxis[i*skip]=0; info->m_J2angularAxis[i*skip+1]=0; info->m_J2angularAxis[i*skip+2]=0; @@ -384,6 +388,10 @@ void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransf info->m_J1linearAxis[0] = 1; info->m_J1linearAxis[skip + 1] = 1; info->m_J1linearAxis[2 * skip + 2] = 1; + + info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[skip + 1] = -1; + info->m_J2linearAxis[2 * skip + 2] = -1; } @@ -702,8 +710,8 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info btTransform trA = transA*m_rbAFrame; btTransform trB = transB*m_rbBFrame; // pivot point - btVector3 pivotAInW = trA.getOrigin(); - btVector3 pivotBInW = trB.getOrigin(); +// btVector3 pivotAInW = trA.getOrigin(); +// btVector3 pivotBInW = trB.getOrigin(); #if 1 // difference between frames in WCS btVector3 ofs = trB.getOrigin() - trA.getOrigin(); @@ -797,7 +805,11 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i]; for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i]; for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i]; - + + for (i=0; i<3; i++) info->m_J2linearAxis[s0+i] = -p[i]; + for (i=0; i<3; i++) info->m_J2linearAxis[s1+i] = -q[i]; + for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -ax1[i]; + // compute three elements of right hand side btScalar rhs = k * p.dot(ofs); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h index cb2973e1d1d..a7f2cca5500 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btHingeConstraint.h @@ -100,6 +100,8 @@ public: public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB, const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA = false); btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA = false); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h index f1994a2dfd8..125580d1998 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btJacobianEntry.h @@ -16,8 +16,7 @@ subject to the following restrictions: #ifndef BT_JACOBIAN_ENTRY_H #define BT_JACOBIAN_ENTRY_H -#include "LinearMath/btVector3.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "LinearMath/btMatrix3x3.h" //notes: diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp index 7e0d93b9765..3c0430b903f 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.cpp @@ -116,15 +116,14 @@ void btPoint2PointConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2); } - /*info->m_J2linearAxis[0] = -1; - info->m_J2linearAxis[s+1] = -1; - info->m_J2linearAxis[2*s+2] = -1; - */ + info->m_J2linearAxis[0] = -1; + info->m_J2linearAxis[info->rowskip+1] = -1; + info->m_J2linearAxis[2*info->rowskip+2] = -1; btVector3 a2 = body1_trans.getBasis()*getPivotInB(); { - btVector3 a2n = -a2; + // btVector3 a2n = -a2; btVector3* angular0 = (btVector3*)(info->m_J2angularAxis); btVector3* angular1 = (btVector3*)(info->m_J2angularAxis+info->rowskip); btVector3* angular2 = (btVector3*)(info->m_J2angularAxis+2*info->rowskip); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h index b3bda03eec1..1e13416dfeb 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h @@ -67,6 +67,8 @@ public: public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + ///for backwards compatibility during the transition to 'getInfo/getInfo2' bool m_useSolveConstraintObsolete; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index ab074224028..0ccadea7ab8 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -14,28 +14,29 @@ subject to the following restrictions: */ //#define COMPUTE_IMPULSE_DENOM 1 +//#define BT_ADDITIONAL_DEBUG + //It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms. #include "btSequentialImpulseConstraintSolver.h" #include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" -#include "btContactConstraint.h" -#include "btSolve2LinearConstraint.h" -#include "btContactSolverInfo.h" + #include "LinearMath/btIDebugDraw.h" -#include "btJacobianEntry.h" +//#include "btJacobianEntry.h" #include "LinearMath/btMinMax.h" #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" #include #include "LinearMath/btStackAlloc.h" #include "LinearMath/btQuickprof.h" -#include "btSolverBody.h" -#include "btSolverConstraint.h" +//#include "btSolverBody.h" +//#include "btSolverConstraint.h" #include "LinearMath/btAlignedObjectArray.h" #include //for memset int gNumSplitImpulseRecoveries = 0; +#include "BulletDynamics/Dynamics/btRigidBody.h" + btSequentialImpulseConstraintSolver::btSequentialImpulseConstraintSolver() :m_btSeed2(0) { @@ -57,15 +58,15 @@ static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 ) #endif//USE_SIMD // Project Gauss Seidel or the equivalent Sequential Impulse -void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) +void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); @@ -78,12 +79,12 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( __m128 upperMinApplied = _mm_sub_ps(upperLimit1,cpAppliedImp); deltaImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, deltaImpulse), _mm_andnot_ps(resultUpperLess, upperMinApplied) ); c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultUpperLess, c.m_appliedImpulse), _mm_andnot_ps(resultUpperLess, upperLimit1) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal2).mVec128,body2.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); #else resolveSingleConstraintRowGeneric(body1,body2,c); @@ -91,11 +92,11 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( } // Project Gauss Seidel or the equivalent Sequential Impulse - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); // const btScalar delta_rel_vel = deltaVel1Dotn-deltaVel2Dotn; deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; @@ -116,19 +117,20 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( { c.m_appliedImpulse = sum; } - body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + + body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); } - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD __m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse); __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhs), _mm_mul_ps(_mm_set1_ps(c.m_appliedImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetDeltaLinearVelocity().mVec128)); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetDeltaAngularVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetDeltaLinearVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetDeltaAngularVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); @@ -138,12 +140,12 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); c.m_appliedImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; body1.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.internalGetDeltaLinearVelocity().mVec128 = _mm_sub_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); #else resolveSingleConstraintRowLowerLimit(body1,body2,c); @@ -151,11 +153,11 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( } // Project Gauss Seidel or the equivalent Sequential Impulse - void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) + void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); - const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetDeltaAngularVelocity()); deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; @@ -169,22 +171,22 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD( { c.m_appliedImpulse = sum; } - body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); } void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly( - btRigidBody& body1, - btRigidBody& body2, + btSolverBody& body1, + btSolverBody& body2, const btSolverConstraint& c) { if (c.m_rhsPenetration) { gNumSplitImpulseRecoveries++; btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm; - const btScalar deltaVel1Dotn = c.m_contactNormal.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity()); - const btScalar deltaVel2Dotn = -c.m_contactNormal.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity()); + const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity()); + const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity()); deltaImpulse -= deltaVel1Dotn*c.m_jacDiagABInv; deltaImpulse -= deltaVel2Dotn*c.m_jacDiagABInv; @@ -198,12 +200,12 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri { c.m_appliedPushImpulse = sum; } - body1.internalApplyPushImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); - body2.internalApplyPushImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); + body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse); + body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse); } } - void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& c) + void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c) { #ifdef USE_SIMD if (!c.m_rhsPenetration) @@ -215,8 +217,8 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri __m128 lowerLimit1 = _mm_set1_ps(c.m_lowerLimit); __m128 upperLimit1 = _mm_set1_ps(c.m_upperLimit); __m128 deltaImpulse = _mm_sub_ps(_mm_set1_ps(c.m_rhsPenetration), _mm_mul_ps(_mm_set1_ps(c.m_appliedPushImpulse),_mm_set1_ps(c.m_cfm))); - __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128)); - __m128 deltaVel2Dotn = _mm_sub_ps(btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128),btSimdDot3((c.m_contactNormal).mVec128,body2.internalGetPushVelocity().mVec128)); + __m128 deltaVel1Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal1.mVec128,body1.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos1CrossNormal.mVec128,body1.internalGetTurnVelocity().mVec128)); + __m128 deltaVel2Dotn = _mm_add_ps(btSimdDot3(c.m_contactNormal2.mVec128,body2.internalGetPushVelocity().mVec128), btSimdDot3(c.m_relpos2CrossNormal.mVec128,body2.internalGetTurnVelocity().mVec128)); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel1Dotn,_mm_set1_ps(c.m_jacDiagABInv))); deltaImpulse = _mm_sub_ps(deltaImpulse,_mm_mul_ps(deltaVel2Dotn,_mm_set1_ps(c.m_jacDiagABInv))); btSimdScalar sum = _mm_add_ps(cpAppliedImp,deltaImpulse); @@ -226,12 +228,12 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri __m128 lowMinApplied = _mm_sub_ps(lowerLimit1,cpAppliedImp); deltaImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowMinApplied), _mm_andnot_ps(resultLowerLess, deltaImpulse) ); c.m_appliedPushImpulse = _mm_or_ps( _mm_and_ps(resultLowerLess, lowerLimit1), _mm_andnot_ps(resultLowerLess, sum) ); - __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal.mVec128,body1.internalGetInvMass().mVec128); - __m128 linearComponentB = _mm_mul_ps((c.m_contactNormal).mVec128,body2.internalGetInvMass().mVec128); + __m128 linearComponentA = _mm_mul_ps(c.m_contactNormal1.mVec128,body1.internalGetInvMass().mVec128); + __m128 linearComponentB = _mm_mul_ps(c.m_contactNormal2.mVec128,body2.internalGetInvMass().mVec128); __m128 impulseMagnitude = deltaImpulse; body1.internalGetPushVelocity().mVec128 = _mm_add_ps(body1.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentA,impulseMagnitude)); body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude)); - body2.internalGetPushVelocity().mVec128 = _mm_sub_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); + body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude)); body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude)); #else resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c); @@ -277,9 +279,10 @@ int btSequentialImpulseConstraintSolver::btRandInt2 (int n) } -#if 0 + void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject) { + btRigidBody* rb = collisionObject? btRigidBody::upcast(collisionObject) : 0; solverBody->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); @@ -289,17 +292,27 @@ void btSequentialImpulseConstraintSolver::initSolverBody(btSolverBody* solverBod if (rb) { - solverBody->internalGetInvMass() = btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor(); + solverBody->m_worldTransform = rb->getWorldTransform(); + solverBody->internalSetInvMass(btVector3(rb->getInvMass(),rb->getInvMass(),rb->getInvMass())*rb->getLinearFactor()); solverBody->m_originalBody = rb; solverBody->m_angularFactor = rb->getAngularFactor(); + solverBody->m_linearFactor = rb->getLinearFactor(); + solverBody->m_linearVelocity = rb->getLinearVelocity(); + solverBody->m_angularVelocity = rb->getAngularVelocity(); } else { - solverBody->internalGetInvMass().setValue(0,0,0); + solverBody->m_worldTransform.setIdentity(); + solverBody->internalSetInvMass(btVector3(0,0,0)); solverBody->m_originalBody = 0; solverBody->m_angularFactor.setValue(1,1,1); + solverBody->m_linearFactor.setValue(1,1,1); + solverBody->m_linearVelocity.setValue(0,0,0); + solverBody->m_angularVelocity.setValue(0,0,0); } + + } -#endif + @@ -313,10 +326,12 @@ btScalar btSequentialImpulseConstraintSolver::restitutionCurve(btScalar rel_vel, -void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection); -void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection) +static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode); +static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirection, int frictionMode) { - if (colObj && colObj->hasAnisotropicFriction()) + + + if (colObj && colObj->hasAnisotropicFriction(frictionMode)) { // transform to local coordinates btVector3 loc_lateral = frictionDirection * colObj->getWorldTransform().getBasis(); @@ -326,20 +341,26 @@ void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& frictionDirec // ... and transform it back to global coordinates frictionDirection = colObj->getWorldTransform().getBasis() * loc_lateral; } + } -void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) -{ - btRigidBody* body0=btRigidBody::upcast(colObj0); - btRigidBody* body1=btRigidBody::upcast(colObj1); +void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) +{ + + + solverConstraint.m_contactNormal1 = normalAxis; + solverConstraint.m_contactNormal2 = -normalAxis; + btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; - solverConstraint.m_contactNormal = normalAxis; + btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; + btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; - solverConstraint.m_solverBodyA = body0 ? body0 : &getFixedBody(); - solverConstraint.m_solverBodyB = body1 ? body1 : &getFixedBody(); + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_friction = cp.m_combinedFriction; solverConstraint.m_originalContactPoint = 0; @@ -348,55 +369,122 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr solverConstraint.m_appliedPushImpulse = 0.f; { - btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal); + btVector3 ftorqueAxis1 = rel_pos1.cross(solverConstraint.m_contactNormal1); solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0); } { - btVector3 ftorqueAxis1 = rel_pos2.cross(-solverConstraint.m_contactNormal); + btVector3 ftorqueAxis1 = rel_pos2.cross(solverConstraint.m_contactNormal2); solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0); } -#ifdef COMPUTE_IMPULSE_DENOM - btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal); - btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal); -#else - btVector3 vec; - btScalar denom0 = 0.f; - btScalar denom1 = 0.f; - if (body0) { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = body0->getInvMass() + normalAxis.dot(vec); + btVector3 vec; + btScalar denom0 = 0.f; + btScalar denom1 = 0.f; + if (body0) + { + vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); + denom0 = body0->getInvMass() + normalAxis.dot(vec); + } + if (body1) + { + vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); + denom1 = body1->getInvMass() + normalAxis.dot(vec); + } + btScalar denom = relaxation/(denom0+denom1); + solverConstraint.m_jacDiagABInv = denom; } - if (body1) + { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = body1->getInvMass() + normalAxis.dot(vec); + + + btScalar rel_vel; + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); + + rel_vel = vel1Dotn+vel2Dotn; + +// btScalar positionalError = 0.f; + + btSimdScalar velocityError = desiredVelocity - rel_vel; + btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv); + solverConstraint.m_rhs = velocityImpulse; + solverConstraint.m_cfm = cfmSlip; + solverConstraint.m_lowerLimit = 0; + solverConstraint.m_upperLimit = 1e10f; + } +} +btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) +{ + btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); + solverConstraint.m_frictionIndex = frictionIndex; + setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, + colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); + return solverConstraint; +} -#endif //COMPUTE_IMPULSE_DENOM - btScalar denom = relaxation/(denom0+denom1); - solverConstraint.m_jacDiagABInv = denom; -#ifdef _USE_JACOBIAN - solverConstraint.m_jac = btJacobianEntry ( - rel_pos1,rel_pos2,solverConstraint.m_contactNormal, - body0->getInvInertiaDiagLocal(), - body0->getInvMass(), - body1->getInvInertiaDiagLocal(), - body1->getInvMass()); -#endif //_USE_JACOBIAN +void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB, + btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, + btScalar desiredVelocity, btScalar cfmSlip) +{ + btVector3 normalAxis(0,0,0); + + + solverConstraint.m_contactNormal1 = normalAxis; + solverConstraint.m_contactNormal2 = -normalAxis; + btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB]; + + btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody; + btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody; + + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + + solverConstraint.m_friction = cp.m_combinedRollingFriction; + solverConstraint.m_originalContactPoint = 0; + + solverConstraint.m_appliedImpulse = 0.f; + solverConstraint.m_appliedPushImpulse = 0.f; + + { + btVector3 ftorqueAxis1 = -normalAxis1; + solverConstraint.m_relpos1CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0); + } + { + btVector3 ftorqueAxis1 = normalAxis1; + solverConstraint.m_relpos2CrossNormal = ftorqueAxis1; + solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0); + } + + + { + btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0); + btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0); + btScalar sum = 0; + sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); + sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); + solverConstraint.m_jacDiagABInv = btScalar(1.)/sum; + } { + + btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?body0->getLinearVelocity():btVector3(0,0,0)) - + solverConstraint.m_relpos1CrossNormal.dot(body0?body0->getAngularVelocity():btVector3(0,0,0)); - btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?body1->getLinearVelocity():btVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(body1?body1->getAngularVelocity():btVector3(0,0,0)); + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0)); + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0)); rel_vel = vel1Dotn+vel2Dotn; @@ -408,33 +496,42 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr solverConstraint.m_cfm = cfmSlip; solverConstraint.m_lowerLimit = 0; solverConstraint.m_upperLimit = 1e10f; + } } -btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) + + + + + +btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) { - btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing(); + btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; - setupFrictionConstraint(solverConstraint, normalAxis, solverBodyA, solverBodyB, cp, rel_pos1, rel_pos2, + setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); return solverConstraint; } + int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body) { -#if 0 + int solverBodyIdA = -1; if (body.getCompanionId() >= 0) { //body has already been converted solverBodyIdA = body.getCompanionId(); + btAssert(solverBodyIdA < m_tmpSolverBodyPool.size()); } else { btRigidBody* rb = btRigidBody::upcast(&body); - if (rb && rb->getInvMass()) + //convert both active and kinematic objects (for their velocity) + if (rb && (rb->getInvMass() || rb->isKinematicObject())) { solverBodyIdA = m_tmpSolverBodyPool.size(); btSolverBody& solverBody = m_tmpSolverBodyPool.expand(); @@ -445,29 +542,33 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& return 0;//assume first one is a fixed solver body } } + return solverBodyIdA; -#endif - return 0; + } #include void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstraint& solverConstraint, - btCollisionObject* colObj0, btCollisionObject* colObj1, + int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation, btVector3& rel_pos1, btVector3& rel_pos2) { - btRigidBody* rb0 = btRigidBody::upcast(colObj0); - btRigidBody* rb1 = btRigidBody::upcast(colObj1); - + const btVector3& pos1 = cp.getPositionWorldOnA(); const btVector3& pos2 = cp.getPositionWorldOnB(); + btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + + btRigidBody* rb0 = bodyA->m_originalBody; + btRigidBody* rb1 = bodyB->m_originalBody; + // btVector3 rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); // btVector3 rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); - rel_pos1 = pos1 - colObj0->getWorldTransform().getOrigin(); - rel_pos2 = pos2 - colObj1->getWorldTransform().getOrigin(); + rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); + rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin(); relaxation = 1.f; @@ -500,30 +601,29 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra solverConstraint.m_jacDiagABInv = denom; } - solverConstraint.m_contactNormal = cp.m_normalWorldOnB; - solverConstraint.m_relpos1CrossNormal = rel_pos1.cross(cp.m_normalWorldOnB); - solverConstraint.m_relpos2CrossNormal = rel_pos2.cross(-cp.m_normalWorldOnB); - + solverConstraint.m_contactNormal1 = cp.m_normalWorldOnB; + solverConstraint.m_contactNormal2 = -cp.m_normalWorldOnB; + solverConstraint.m_relpos1CrossNormal = torqueAxis0; + solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + btScalar restitution = 0.f; + btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; + { + btVector3 vel1,vel2; - btVector3 vel1 = rb0 ? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); - btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - vel = vel1 - vel2; - rel_vel = cp.m_normalWorldOnB.dot(vel); + vel1 = rb0? rb0->getVelocityInLocalPoint(rel_pos1) : btVector3(0,0,0); + vel2 = rb1? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); - btScalar penetration = cp.getDistance()+infoGlobal.m_linearSlop; + // btVector3 vel2 = rb1 ? rb1->getVelocityInLocalPoint(rel_pos2) : btVector3(0,0,0); + vel = vel1 - vel2; + rel_vel = cp.m_normalWorldOnB.dot(vel); + - solverConstraint.m_friction = cp.m_combinedFriction; + solverConstraint.m_friction = cp.m_combinedFriction; - btScalar restitution = 0.f; - if (cp.m_lifeTime>infoGlobal.m_restingContactRestitutionThreshold) - { - restitution = 0.f; - } else - { restitution = restitutionCurve(rel_vel, cp.m_combinedRestitution); if (restitution <= btScalar(0.)) { @@ -537,9 +637,9 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra { solverConstraint.m_appliedImpulse = cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor; if (rb0) - rb0->internalApplyImpulse(solverConstraint.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); + bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse); if (rb1) - rb1->internalApplyImpulse(solverConstraint.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); + bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse); } else { solverConstraint.m_appliedImpulse = 0.f; @@ -548,33 +648,41 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra solverConstraint.m_appliedPushImpulse = 0.f; { - btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rb0?rb0->getLinearVelocity():btVector3(0,0,0)) - + solverConstraint.m_relpos1CrossNormal.dot(rb0?rb0->getAngularVelocity():btVector3(0,0,0)); - btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rb1?rb1->getLinearVelocity():btVector3(0,0,0)) - + solverConstraint.m_relpos2CrossNormal.dot(rb1?rb1->getAngularVelocity():btVector3(0,0,0)); - - rel_vel = vel1Dotn+vel2Dotn; + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rb0?bodyA->m_linearVelocity:btVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(rb0?bodyA->m_angularVelocity:btVector3(0,0,0)); + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rb1?bodyB->m_linearVelocity:btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(rb1?bodyB->m_angularVelocity:btVector3(0,0,0)); + btScalar rel_vel = vel1Dotn+vel2Dotn; btScalar positionalError = 0.f; btScalar velocityError = restitution - rel_vel;// * damping; + + + btScalar erp = infoGlobal.m_erp2; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) + { + erp = infoGlobal.m_erp; + } if (penetration>0) { positionalError = 0; + velocityError -= penetration / infoGlobal.m_timeStep; } else { - positionalError = -penetration * infoGlobal.m_erp/infoGlobal.m_timeStep; + positionalError = -penetration * erp/infoGlobal.m_timeStep; } btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv; + if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) { //combine position and velocity into rhs solverConstraint.m_rhs = penetrationImpulse+velocityImpulse; solverConstraint.m_rhsPenetration = 0.f; + } else { //split position and velocity into rhs and m_rhsPenetration @@ -594,51 +702,46 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra void btSequentialImpulseConstraintSolver::setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, - btRigidBody* rb0, btRigidBody* rb1, + int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal) { - if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) - { - { - btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; - if (rb0) - rb0->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); - if (rb1) - rb1->internalApplyImpulse(frictionConstraint1.m_contactNormal*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse); - } else - { - frictionConstraint1.m_appliedImpulse = 0.f; - } - } - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) - { - frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; - if (rb0) - rb0->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); - if (rb1) - rb1->internalApplyImpulse(frictionConstraint2.m_contactNormal*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse); - } else - { - frictionConstraint2.m_appliedImpulse = 0.f; - } - } - } else - { - btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; - frictionConstraint1.m_appliedImpulse = 0.f; - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; - frictionConstraint2.m_appliedImpulse = 0.f; - } - } + btSolverBody* bodyA = &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* bodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + + btRigidBody* rb0 = bodyA->m_originalBody; + btRigidBody* rb1 = bodyB->m_originalBody; + + { + btSolverConstraint& frictionConstraint1 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex]; + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + frictionConstraint1.m_appliedImpulse = cp.m_appliedImpulseLateral1 * infoGlobal.m_warmstartingFactor; + if (rb0) + bodyA->internalApplyImpulse(frictionConstraint1.m_contactNormal1*rb0->getInvMass()*rb0->getLinearFactor(),frictionConstraint1.m_angularComponentA,frictionConstraint1.m_appliedImpulse); + if (rb1) + bodyB->internalApplyImpulse(-frictionConstraint1.m_contactNormal2*rb1->getInvMass()*rb1->getLinearFactor(),-frictionConstraint1.m_angularComponentB,-(btScalar)frictionConstraint1.m_appliedImpulse); + } else + { + frictionConstraint1.m_appliedImpulse = 0.f; + } + } + + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + btSolverConstraint& frictionConstraint2 = m_tmpSolverContactFrictionConstraintPool[solverConstraint.m_frictionIndex+1]; + if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + { + frictionConstraint2.m_appliedImpulse = cp.m_appliedImpulseLateral2 * infoGlobal.m_warmstartingFactor; + if (rb0) + bodyA->internalApplyImpulse(frictionConstraint2.m_contactNormal1*rb0->getInvMass(),frictionConstraint2.m_angularComponentA,frictionConstraint2.m_appliedImpulse); + if (rb1) + bodyB->internalApplyImpulse(-frictionConstraint2.m_contactNormal2*rb1->getInvMass(),-frictionConstraint2.m_angularComponentB,-(btScalar)frictionConstraint2.m_appliedImpulse); + } else + { + frictionConstraint2.m_appliedImpulse = 0.f; + } + } } @@ -651,14 +754,22 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m colObj0 = (btCollisionObject*)manifold->getBody0(); colObj1 = (btCollisionObject*)manifold->getBody1(); + int solverBodyIdA = getOrInitSolverBody(*colObj0); + int solverBodyIdB = getOrInitSolverBody(*colObj1); + +// btRigidBody* bodyA = btRigidBody::upcast(colObj0); +// btRigidBody* bodyB = btRigidBody::upcast(colObj1); + + btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverBodyIdB]; + - btRigidBody* solverBodyA = btRigidBody::upcast(colObj0); - btRigidBody* solverBodyB = btRigidBody::upcast(colObj1); ///avoid collision response between two static objects - if ((!solverBodyA || !solverBodyA->getInvMass()) && (!solverBodyB || !solverBodyB->getInvMass())) + if (!solverBodyA || (!solverBodyA->m_originalBody && (!solverBodyB || !solverBodyB->m_originalBody))) return; + int rollingFriction=1; for (int j=0;jgetNumContacts();j++) { @@ -674,13 +785,14 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m int frictionIndex = m_tmpSolverContactConstraintPool.size(); btSolverConstraint& solverConstraint = m_tmpSolverContactConstraintPool.expandNonInitializing(); - btRigidBody* rb0 = btRigidBody::upcast(colObj0); - btRigidBody* rb1 = btRigidBody::upcast(colObj1); - solverConstraint.m_solverBodyA = rb0? rb0 : &getFixedBody(); - solverConstraint.m_solverBodyB = rb1? rb1 : &getFixedBody(); +// btRigidBody* rb0 = btRigidBody::upcast(colObj0); +// btRigidBody* rb1 = btRigidBody::upcast(colObj1); + solverConstraint.m_solverBodyIdA = solverBodyIdA; + solverConstraint.m_solverBodyIdB = solverBodyIdB; + solverConstraint.m_originalContactPoint = &cp; - setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2); + setupContactConstraint(solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2); // const btVector3& pos1 = cp.getPositionWorldOnA(); // const btVector3& pos2 = cp.getPositionWorldOnB(); @@ -689,52 +801,109 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); + btVector3 angVelA,angVelB; + solverBodyA->getAngularVelocity(angVelA); + solverBodyB->getAngularVelocity(angVelB); + btVector3 relAngVel = angVelB-angVelA; + + if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) + { + //only a single rollingFriction per manifold + rollingFriction--; + if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) + { + relAngVel.normalize(); + applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + if (relAngVel.length()>0.001) + addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } else + { + addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + btVector3 axis0,axis1; + btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); + applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + if (axis0.length()>0.001) + addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + if (axis1.length()>0.001) + addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + + } + } + + ///Bullet has several options to set the friction directions + ///By default, each contact has only a single friction direction that is recomputed automatically very frame + ///based on the relative linear velocity. + ///If the relative velocity it zero, it will automatically compute a friction direction. + + ///You can also enable two friction directions, using the SOLVER_USE_2_FRICTION_DIRECTIONS. + ///In that case, the second friction direction will be orthogonal to both contact normal and first friction direction. + /// + ///If you choose SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION, then the friction will be independent from the relative projected velocity. + /// + ///The user can manually override the friction directions for certain contacts using a contact callback, + ///and set the cp.m_lateralFrictionInitialized to true + ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) + ///this will give a conveyor belt effect + /// if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) { cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); if (!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON) { - cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel); + cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel); if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB); cp.m_lateralFrictionDir2.normalize();//?? - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + } - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - cp.m_lateralFrictionInitialized = true; + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + } else { - //re-calculate friction direction every frame, todo: check if this is really needed btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2); - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } - applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1); - applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1); - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - cp.m_lateralFrictionInitialized = true; + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) + { + cp.m_lateralFrictionInitialized = true; + } } } else { - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); + + setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); } + + - setFrictionConstraintImpulse( solverConstraint, rb0, rb1, cp, infoGlobal); } } @@ -748,39 +917,107 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol m_maxOverrideNumSolverIterations = 0; - if (!(numConstraints + numManifolds)) - { - // printf("empty\n"); - return 0.f; - } - - if (infoGlobal.m_splitImpulse) +#ifdef BT_ADDITIONAL_DEBUG + //make sure that dynamic bodies exist for all (enabled) constraints + for (int i=0;iisEnabled()) { - btRigidBody* body = btRigidBody::upcast(bodies[i]); - if (body) - { - body->internalGetDeltaLinearVelocity().setZero(); - body->internalGetDeltaAngularVelocity().setZero(); - body->internalGetPushVelocity().setZero(); - body->internalGetTurnVelocity().setZero(); + if (!constraint->getRigidBodyA().isStaticOrKinematicObject()) + { + bool found=false; + for (int b=0;bgetRigidBodyA()==bodies[b]) + { + found = true; + break; + } + } + btAssert(found); + } + if (!constraint->getRigidBodyB().isStaticOrKinematicObject()) + { + bool found=false; + for (int b=0;bgetRigidBodyB()==bodies[b]) + { + found = true; + break; + } + } + btAssert(found); } } } - else + //make sure that dynamic bodies exist for all contact manifolds + for (int i=0;igetBody0()->isStaticOrKinematicObject()) + { + bool found=false; + for (int b=0;bgetBody0()==bodies[b]) + { + found = true; + break; + } + } + btAssert(found); + } + if (!manifoldPtr[i]->getBody1()->isStaticOrKinematicObject()) + { + bool found=false; + for (int b=0;bgetBody1()==bodies[b]) + { + found = true; + break; + } + } + btAssert(found); + } + } +#endif //BT_ADDITIONAL_DEBUG + + + for (int i = 0; i < numBodies; i++) { - for (int i = 0; i < numBodies; i++) + bodies[i]->setCompanionId(-1); + } + + + m_tmpSolverBodyPool.reserve(numBodies+1); + m_tmpSolverBodyPool.resize(0); + + btSolverBody& fixedBody = m_tmpSolverBodyPool.expand(); + initSolverBody(&fixedBody,0); + + //convert all bodies + + for (int i=0;igetInvMass()) { - btRigidBody* body = btRigidBody::upcast(bodies[i]); - if (body) - { - body->internalGetDeltaLinearVelocity().setZero(); - body->internalGetDeltaAngularVelocity().setZero(); + btSolverBody& solverBody = m_tmpSolverBodyPool[bodyId]; + btVector3 gyroForce (0,0,0); + if (body->getFlags()&BT_ENABLE_GYROPSCOPIC_FORCE) + { + gyroForce = body->computeGyroscopicForce(infoGlobal.m_maxGyroscopicForce); } + solverBody.m_linearVelocity += body->getTotalForce()*body->getInvMass()*infoGlobal.m_timeStep; + solverBody.m_angularVelocity += (body->getTotalTorque()-gyroForce)*body->getInvInertiaTensorWorld()*infoGlobal.m_timeStep; } } - + if (1) { int j; @@ -791,6 +1028,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol constraint->internalSetAppliedImpulse(0.0f); } } + //btRigidBody* rb0=0,*rb1=0; //if (1) @@ -800,11 +1038,23 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol int totalNumRows = 0; int i; - m_tmpConstraintSizesPool.resize(numConstraints); + m_tmpConstraintSizesPool.resizeNoInitialize(numConstraints); //calculate the total number of contraint rows for (i=0;igetJointFeedback(); + if (fb) + { + fb->m_appliedForceBodyA.setZero(); + fb->m_appliedTorqueBodyA.setZero(); + fb->m_appliedForceBodyB.setZero(); + fb->m_appliedTorqueBodyB.setZero(); + } + + if (constraints[i]->isEnabled()) + { + } if (constraints[i]->isEnabled()) { constraints[i]->getInfo1(&info1); @@ -815,7 +1065,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } totalNumRows += info1.m_numConstraintRows; } - m_tmpSolverNonContactConstraintPool.resize(totalNumRows); + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(totalNumRows); ///setup the btSolverConstraints @@ -834,6 +1084,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol btRigidBody& rbA = constraint->getRigidBodyA(); btRigidBody& rbB = constraint->getRigidBodyB(); + int solverBodyIdA = getOrInitSolverBody(rbA); + int solverBodyIdB = getOrInitSolverBody(rbB); + + btSolverBody* bodyAPtr = &m_tmpSolverBodyPool[solverBodyIdA]; + btSolverBody* bodyBPtr = &m_tmpSolverBodyPool[solverBodyIdB]; + + + int overrideNumSolverIterations = constraint->getOverrideNumSolverIterations() > 0 ? constraint->getOverrideNumSolverIterations() : infoGlobal.m_numIterations; if (overrideNumSolverIterations>m_maxOverrideNumSolverIterations) @@ -848,28 +1106,31 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol currentConstraintRow[j].m_upperLimit = SIMD_INFINITY; currentConstraintRow[j].m_appliedImpulse = 0.f; currentConstraintRow[j].m_appliedPushImpulse = 0.f; - currentConstraintRow[j].m_solverBodyA = &rbA; - currentConstraintRow[j].m_solverBodyB = &rbB; + currentConstraintRow[j].m_solverBodyIdA = solverBodyIdA; + currentConstraintRow[j].m_solverBodyIdB = solverBodyIdB; currentConstraintRow[j].m_overrideNumSolverIterations = overrideNumSolverIterations; } - rbA.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - rbA.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - rbB.internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); - rbB.internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); - + bodyAPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + bodyAPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + bodyAPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); + bodyAPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetDeltaLinearVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetDeltaAngularVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetPushVelocity().setValue(0.f,0.f,0.f); + bodyBPtr->internalGetTurnVelocity().setValue(0.f,0.f,0.f); btTypedConstraint::btConstraintInfo2 info2; info2.fps = 1.f/infoGlobal.m_timeStep; info2.erp = infoGlobal.m_erp; - info2.m_J1linearAxis = currentConstraintRow->m_contactNormal; + info2.m_J1linearAxis = currentConstraintRow->m_contactNormal1; info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal; - info2.m_J2linearAxis = 0; + info2.m_J2linearAxis = currentConstraintRow->m_contactNormal2; info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal; info2.rowskip = sizeof(btSolverConstraint)/sizeof(btScalar);//check this ///the size of btSolverConstraint needs be a multiple of btScalar - btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); + btAssert(info2.rowskip*sizeof(btScalar)== sizeof(btSolverConstraint)); info2.m_constraintError = ¤tConstraintRow->m_rhs; currentConstraintRow->m_cfm = infoGlobal.m_globalCfm; info2.m_damping = infoGlobal.m_damping; @@ -906,17 +1167,18 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } { - btVector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass(); + btVector3 iMJlA = solverConstraint.m_contactNormal1*rbA.getInvMass(); btVector3 iMJaA = rbA.getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal; - btVector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal? + btVector3 iMJlB = solverConstraint.m_contactNormal2*rbB.getInvMass();//sign of normal? btVector3 iMJaB = rbB.getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal; - btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal); + btScalar sum = iMJlA.dot(solverConstraint.m_contactNormal1); sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal); - sum += iMJlB.dot(solverConstraint.m_contactNormal); + sum += iMJlB.dot(solverConstraint.m_contactNormal2); sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal); - - solverConstraint.m_jacDiagABInv = btScalar(1.)/sum; + btScalar fsum = btFabs(sum); + btAssert(fsum > SIMD_EPSILON); + solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f; } @@ -924,8 +1186,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol ///todo: add force/torque accelerators { btScalar rel_vel; - btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()); - btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()); + btScalar vel1Dotn = solverConstraint.m_contactNormal1.dot(rbA.getLinearVelocity()) + solverConstraint.m_relpos1CrossNormal.dot(rbA.getAngularVelocity()); + btScalar vel2Dotn = solverConstraint.m_contactNormal2.dot(rbB.getLinearVelocity()) + solverConstraint.m_relpos2CrossNormal.dot(rbB.getAngularVelocity()); rel_vel = vel1Dotn+vel2Dotn; @@ -958,7 +1220,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol } } - btContactSolverInfo info = infoGlobal; +// btContactSolverInfo info = infoGlobal; int numNonContactPool = m_tmpSolverNonContactConstraintPool.size(); @@ -966,9 +1228,13 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size(); ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints - m_orderNonContactConstraintPool.resize(numNonContactPool); - m_orderTmpConstraintPool.resize(numConstraintPool); - m_orderFrictionConstraintPool.resize(numFrictionPool); + m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool*2); + else + m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool); + + m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool); { int i; for (i=0;isolveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + if (constraints[j]->isEnabled()) + { + int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); + btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; + constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); + } } ///solve all contact constraints using SIMD, if available - int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (j=0;jbtScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + } + } + + if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) + { + + btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]]; + + if (totalImpulse>btScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + } + } + } + } } - - ///solve all friction constraints, using SIMD, if available - int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size(); - for (j=0;jbtScalar(0)) + for (j=0;jbtScalar(0)) + { + solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse); + solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse; + + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); + } + } + + + int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size(); + for (j=0;jbtScalar(0)) + { + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; + if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); + } + } + + + } } } else { - + //non-SIMD version ///solve all joint constraints - for (j=0;jsolveConstraintObsolete(constraints[j]->getRigidBodyA(),constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep); + if (constraints[j]->isEnabled()) + { + int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA()); + int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB()); + btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid]; + btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid]; + constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep); + } } ///solve all contact constraints int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); - for (j=0;jbtScalar(0)) + { + btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse; + if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction) + rollingFrictionMagnitude = rollingFrictionConstraint.m_friction; + + rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude; + rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude; + + resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint); } } } @@ -1131,7 +1508,7 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte { const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSplitPenetrationSIMD(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); + resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); } } } @@ -1147,7 +1524,7 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte { const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]]; - resolveSplitPenetrationImpulseCacheFriendly(*solveManifold.m_solverBodyA,*solveManifold.m_solverBodyB,solveManifold); + resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold); } } } @@ -1175,25 +1552,29 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations( return 0.f; } -btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** /*constraints*/,int /* numConstraints*/,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/,btStackAlloc* /*stackAlloc*/) +btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal) { int numPoolConstraints = m_tmpSolverContactConstraintPool.size(); int i,j; - for (j=0;jm_appliedImpulse = solveManifold.m_appliedImpulse; - if (infoGlobal.m_solverMode & SOLVER_USE_FRICTION_WARMSTARTING) + for (j=0;jm_appliedImpulse = solveManifold.m_appliedImpulse; + // float f = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; + // printf("pt->m_appliedImpulseLateral1 = %f\n", f); pt->m_appliedImpulseLateral1 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse; - pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; + //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); + if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + { + pt->m_appliedImpulseLateral2 = m_tmpSolverContactFrictionConstraintPool[solveManifold.m_frictionIndex+1].m_appliedImpulse; + } + //do a callback here? } - - //do a callback here? } numPoolConstraints = m_tmpSolverNonContactConstraintPool.size(); @@ -1201,6 +1582,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo { const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j]; btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint; + btJointFeedback* fb = constr->getJointFeedback(); + if (fb) + { + fb->m_appliedForceBodyA += solverConstr.m_contactNormal1*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep; + fb->m_appliedForceBodyB += solverConstr.m_contactNormal2*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; + fb->m_appliedTorqueBodyB += solverConstr.m_relpos2CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep; /*RGM ???? */ + + } + constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse); if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold()) { @@ -1209,29 +1600,32 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo } - if (infoGlobal.m_splitImpulse) - { - for ( i=0;iinternalWritebackVelocity(infoGlobal.m_timeStep); - } - } else + + for ( i=0;iinternalWritebackVelocity(); + if (infoGlobal.m_splitImpulse) + m_tmpSolverBodyPool[i].writebackVelocityAndTransform(infoGlobal.m_timeStep, infoGlobal.m_splitImpulseTurnErp); + else + m_tmpSolverBodyPool[i].writebackVelocity(); + + m_tmpSolverBodyPool[i].m_originalBody->setLinearVelocity(m_tmpSolverBodyPool[i].m_linearVelocity); + m_tmpSolverBodyPool[i].m_originalBody->setAngularVelocity(m_tmpSolverBodyPool[i].m_angularVelocity); + if (infoGlobal.m_splitImpulse) + m_tmpSolverBodyPool[i].m_originalBody->setWorldTransform(m_tmpSolverBodyPool[i].m_worldTransform); + + m_tmpSolverBodyPool[i].m_originalBody->setCompanionId(-1); } } + m_tmpSolverContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0); + m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0); - m_tmpSolverContactConstraintPool.resize(0); - m_tmpSolverNonContactConstraintPool.resize(0); - m_tmpSolverContactFrictionConstraintPool.resize(0); - + m_tmpSolverBodyPool.resizeNoInitialize(0); return 0.f; } @@ -1243,14 +1637,12 @@ btScalar btSequentialImpulseConstraintSolver::solveGroup(btCollisionObject** bod BT_PROFILE("solveGroup"); //you need to provide at least some bodies - btAssert(bodies); - btAssert(numBodies); - + solveGroupCacheFriendlySetup( bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); solveGroupCacheFriendlyIterations(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); - solveGroupCacheFriendlyFinish(bodies, numBodies, manifoldPtr, numManifolds,constraints, numConstraints,infoGlobal,debugDrawer, stackAlloc); + solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal); return 0.f; } @@ -1260,10 +1652,4 @@ void btSequentialImpulseConstraintSolver::reset() m_btSeed2 = 0; } -btRigidBody& btSequentialImpulseConstraintSolver::getFixedBody() -{ - static btRigidBody s_fixed(0, 0,0); - s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.))); - return s_fixed; -} diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index bb377db8db9..2eea6be0db2 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -16,77 +16,89 @@ subject to the following restrictions: #ifndef BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H #define BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H -#include "btConstraintSolver.h" class btIDebugDraw; -#include "btContactConstraint.h" -#include "btSolverBody.h" -#include "btSolverConstraint.h" -#include "btTypedConstraint.h" +class btPersistentManifold; +class btStackAlloc; +class btDispatcher; +class btCollisionObject; +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h" +#include "BulletDynamics/ConstraintSolver/btSolverBody.h" +#include "BulletDynamics/ConstraintSolver/btSolverConstraint.h" #include "BulletCollision/NarrowPhaseCollision/btManifoldPoint.h" +#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method. -class btSequentialImpulseConstraintSolver : public btConstraintSolver +ATTRIBUTE_ALIGNED16(class) btSequentialImpulseConstraintSolver : public btConstraintSolver { protected: - + btAlignedObjectArray m_tmpSolverBodyPool; btConstraintArray m_tmpSolverContactConstraintPool; btConstraintArray m_tmpSolverNonContactConstraintPool; btConstraintArray m_tmpSolverContactFrictionConstraintPool; + btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool; + btAlignedObjectArray m_orderTmpConstraintPool; btAlignedObjectArray m_orderNonContactConstraintPool; btAlignedObjectArray m_orderFrictionConstraintPool; btAlignedObjectArray m_tmpConstraintSizesPool; int m_maxOverrideNumSolverIterations; - void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyIdB, + void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); - btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,btRigidBody* solverBodyA,btRigidBody* solverBodyB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); - - void setupContactConstraint(btSolverConstraint& solverConstraint, btCollisionObject* colObj0, btCollisionObject* colObj1, btManifoldPoint& cp, + void setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, + btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, + btScalar desiredVelocity=0., btScalar cfmSlip=0.); + + btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); + btSolverConstraint& addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f); + + + void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation, btVector3& rel_pos1, btVector3& rel_pos2); - void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, btRigidBody* rb0, btRigidBody* rb1, + void setFrictionConstraintImpulse( btSolverConstraint& solverConstraint, int solverBodyIdA,int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal); ///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction unsigned long m_btSeed2; -// void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); + btScalar restitutionCurve(btScalar rel_vel, btScalar restitution); void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal); void resolveSplitPenetrationSIMD( - btRigidBody& body1, - btRigidBody& body2, + btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint); void resolveSplitPenetrationImpulseCacheFriendly( - btRigidBody& body1, - btRigidBody& body2, + btSolverBody& bodyA,btSolverBody& bodyB, const btSolverConstraint& contactConstraint); //internal method - int getOrInitSolverBody(btCollisionObject& body); + int getOrInitSolverBody(btCollisionObject& body); + void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject); - void resolveSingleConstraintRowGeneric(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - void resolveSingleConstraintRowGenericSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - void resolveSingleConstraintRowLowerLimit(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); - void resolveSingleConstraintRowLowerLimitSIMD(btRigidBody& body1,btRigidBody& body2,const btSolverConstraint& contactConstraint); + void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint); protected: - static btRigidBody& getFixedBody(); + virtual void solveGroupCacheFriendlySplitImpulseIterations(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); - virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); + virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal); btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer,btStackAlloc* stackAlloc); @@ -95,6 +107,7 @@ protected: public: + BT_DECLARE_ALIGNED_ALLOCATOR(); btSequentialImpulseConstraintSolver(); virtual ~btSequentialImpulseConstraintSolver(); @@ -121,9 +134,7 @@ public: }; -#ifndef BT_PREFER_SIMD -typedef btSequentialImpulseConstraintSolver btSequentialImpulseConstraintSolverPrefered; -#endif + #endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp old mode 100644 new mode 100755 index b69f46da1b4..aff9f27f594 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.cpp @@ -426,6 +426,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra for (i=0; i<3; i++) info->m_J2angularAxis[s3+i] = -tmpB[i]; for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; + for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i]; + for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i]; } else { // old way - maybe incorrect if bodies are not on the slider axis @@ -440,6 +442,8 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = p[i]; for (i=0; i<3; i++) info->m_J1linearAxis[s3+i] = q[i]; + for (i=0; i<3; i++) info->m_J2linearAxis[s2+i] = -p[i]; + for (i=0; i<3; i++) info->m_J2linearAxis[s3+i] = -q[i]; } // compute two elements of right hand side @@ -479,6 +483,9 @@ void btSliderConstraint::getInfo2NonVirtual(btConstraintInfo2* info, const btTra info->m_J1linearAxis[srow+0] = ax1[0]; info->m_J1linearAxis[srow+1] = ax1[1]; info->m_J1linearAxis[srow+2] = ax1[2]; + info->m_J2linearAxis[srow+0] = -ax1[0]; + info->m_J2linearAxis[srow+1] = -ax1[1]; + info->m_J2linearAxis[srow+2] = -ax1[2]; // linear torque decoupling step: // // we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h old mode 100644 new mode 100755 index 2edc8d2b2e1..ca8e715bc47 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSliderConstraint.h @@ -60,7 +60,7 @@ enum btSliderFlags }; -class btSliderConstraint : public btTypedConstraint +ATTRIBUTE_ALIGNED16(class) btSliderConstraint : public btTypedConstraint { protected: ///for backwards compatibility during the transition to 'getInfo/getInfo2' @@ -155,6 +155,8 @@ protected: //------------------------ void initParams(); public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + // constructors btSliderConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA); btSliderConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameA); diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h index 8de515812ee..ccc45996c8b 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverBody.h @@ -19,7 +19,7 @@ subject to the following restrictions: class btRigidBody; #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" -#include "BulletDynamics/Dynamics/btRigidBody.h" + #include "LinearMath/btAlignedAllocator.h" #include "LinearMath/btTransformUtil.h" @@ -105,22 +105,35 @@ operator+(const btSimdScalar& v1, const btSimdScalar& v2) #endif ///The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packed to increase cache coherence/performance. -ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete +ATTRIBUTE_ALIGNED16 (struct) btSolverBody { BT_DECLARE_ALIGNED_ALLOCATOR(); + btTransform m_worldTransform; btVector3 m_deltaLinearVelocity; btVector3 m_deltaAngularVelocity; btVector3 m_angularFactor; + btVector3 m_linearFactor; btVector3 m_invMass; - btRigidBody* m_originalBody; btVector3 m_pushVelocity; btVector3 m_turnVelocity; + btVector3 m_linearVelocity; + btVector3 m_angularVelocity; + + btRigidBody* m_originalBody; + void setWorldTransform(const btTransform& worldTransform) + { + m_worldTransform = worldTransform; + } + const btTransform& getWorldTransform() const + { + return m_worldTransform; + } SIMD_FORCE_INLINE void getVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const { if (m_originalBody) - velocity = m_originalBody->getLinearVelocity()+m_deltaLinearVelocity + (m_originalBody->getAngularVelocity()+m_deltaAngularVelocity).cross(rel_pos); + velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); else velocity.setValue(0,0,0); } @@ -128,7 +141,7 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete SIMD_FORCE_INLINE void getAngularVelocity(btVector3& angVel) const { if (m_originalBody) - angVel = m_originalBody->getAngularVelocity()+m_deltaAngularVelocity; + angVel =m_angularVelocity+m_deltaAngularVelocity; else angVel.setValue(0,0,0); } @@ -137,9 +150,9 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position SIMD_FORCE_INLINE void applyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude) { - //if (m_invMass) + if (m_originalBody) { - m_deltaLinearVelocity += linearComponent*impulseMagnitude; + m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); } } @@ -148,36 +161,125 @@ ATTRIBUTE_ALIGNED64 (struct) btSolverBodyObsolete { if (m_originalBody) { - m_pushVelocity += linearComponent*impulseMagnitude; + m_pushVelocity += linearComponent*impulseMagnitude*m_linearFactor; m_turnVelocity += angularComponent*(impulseMagnitude*m_angularFactor); } } + + + + const btVector3& getDeltaLinearVelocity() const + { + return m_deltaLinearVelocity; + } + + const btVector3& getDeltaAngularVelocity() const + { + return m_deltaAngularVelocity; + } + + const btVector3& getPushVelocity() const + { + return m_pushVelocity; + } + + const btVector3& getTurnVelocity() const + { + return m_turnVelocity; + } + + + //////////////////////////////////////////////// + ///some internal methods, don't use them + + btVector3& internalGetDeltaLinearVelocity() + { + return m_deltaLinearVelocity; + } + + btVector3& internalGetDeltaAngularVelocity() + { + return m_deltaAngularVelocity; + } + + const btVector3& internalGetAngularFactor() const + { + return m_angularFactor; + } + + const btVector3& internalGetInvMass() const + { + return m_invMass; + } + + void internalSetInvMass(const btVector3& invMass) + { + m_invMass = invMass; + } + btVector3& internalGetPushVelocity() + { + return m_pushVelocity; + } + + btVector3& internalGetTurnVelocity() + { + return m_turnVelocity; + } + + SIMD_FORCE_INLINE void internalGetVelocityInLocalPointObsolete(const btVector3& rel_pos, btVector3& velocity ) const + { + velocity = m_linearVelocity+m_deltaLinearVelocity + (m_angularVelocity+m_deltaAngularVelocity).cross(rel_pos); + } + + SIMD_FORCE_INLINE void internalGetAngularVelocity(btVector3& angVel) const + { + angVel = m_angularVelocity+m_deltaAngularVelocity; + } + + + //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position + SIMD_FORCE_INLINE void internalApplyImpulse(const btVector3& linearComponent, const btVector3& angularComponent,const btScalar impulseMagnitude) + { + if (m_originalBody) + { + m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor; + m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor); + } + } + + + + void writebackVelocity() { if (m_originalBody) { - m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity); - m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); + m_linearVelocity +=m_deltaLinearVelocity; + m_angularVelocity += m_deltaAngularVelocity; //m_originalBody->setCompanionId(-1); } } - void writebackVelocity(btScalar timeStep) + void writebackVelocityAndTransform(btScalar timeStep, btScalar splitImpulseTurnErp) { (void) timeStep; if (m_originalBody) { - m_originalBody->setLinearVelocity(m_originalBody->getLinearVelocity()+ m_deltaLinearVelocity); - m_originalBody->setAngularVelocity(m_originalBody->getAngularVelocity()+m_deltaAngularVelocity); + m_linearVelocity += m_deltaLinearVelocity; + m_angularVelocity += m_deltaAngularVelocity; //correct the position/orientation based on push/turn recovery btTransform newTransform; - btTransformUtil::integrateTransform(m_originalBody->getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform); - m_originalBody->setWorldTransform(newTransform); - + if (m_pushVelocity[0]!=0.f || m_pushVelocity[1]!=0 || m_pushVelocity[2]!=0 || m_turnVelocity[0]!=0.f || m_turnVelocity[1]!=0 || m_turnVelocity[2]!=0) + { + // btQuaternion orn = m_worldTransform.getRotation(); + btTransformUtil::integrateTransform(m_worldTransform,m_pushVelocity,m_turnVelocity*splitImpulseTurnErp,timeStep,newTransform); + m_worldTransform = newTransform; + } + //m_worldTransform.setRotation(orn); //m_originalBody->setCompanionId(-1); } } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h index 179e79d7911..2ade61b2f69 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btSolverConstraint.h @@ -20,68 +20,49 @@ class btRigidBody; #include "LinearMath/btVector3.h" #include "LinearMath/btMatrix3x3.h" #include "btJacobianEntry.h" +#include "LinearMath/btAlignedObjectArray.h" //#define NO_FRICTION_TANGENTIALS 1 #include "btSolverBody.h" ///1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints. -ATTRIBUTE_ALIGNED64 (struct) btSolverConstraint +ATTRIBUTE_ALIGNED16 (struct) btSolverConstraint { BT_DECLARE_ALIGNED_ALLOCATOR(); btVector3 m_relpos1CrossNormal; - btVector3 m_contactNormal; + btVector3 m_contactNormal1; btVector3 m_relpos2CrossNormal; - //btVector3 m_contactNormal2;//usually m_contactNormal2 == -m_contactNormal + btVector3 m_contactNormal2; //usually m_contactNormal2 == -m_contactNormal1, but not always btVector3 m_angularComponentA; btVector3 m_angularComponentB; mutable btSimdScalar m_appliedPushImpulse; mutable btSimdScalar m_appliedImpulse; - - + btScalar m_friction; btScalar m_jacDiagABInv; - union - { - int m_numConsecutiveRowsPerKernel; - btScalar m_unusedPadding0; - }; - - int m_overrideNumSolverIterations; - - union - { - int m_frictionIndex; - btScalar m_unusedPadding1; - }; - union - { - btRigidBody* m_solverBodyA; - int m_companionIdA; - }; - union - { - btRigidBody* m_solverBodyB; - int m_companionIdB; - }; + btScalar m_rhs; + btScalar m_cfm; - union + btScalar m_lowerLimit; + btScalar m_upperLimit; + btScalar m_rhsPenetration; + union { void* m_originalContactPoint; btScalar m_unusedPadding4; }; - btScalar m_rhs; - btScalar m_cfm; - btScalar m_lowerLimit; - btScalar m_upperLimit; - - btScalar m_rhsPenetration; + int m_overrideNumSolverIterations; + int m_frictionIndex; + int m_solverBodyIdA; + int m_solverBodyIdB; + enum btSolverConstraintType { BT_SOLVER_CONTACT_1D = 0, diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp index 06bde5e7eec..465c0746c58 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp @@ -32,7 +32,8 @@ m_overrideNumSolverIterations(-1), m_rbA(rbA), m_rbB(getFixedBody()), m_appliedImpulse(btScalar(0.)), -m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) +m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE), +m_jointFeedback(0) { } @@ -48,7 +49,8 @@ m_overrideNumSolverIterations(-1), m_rbA(rbA), m_rbB(rbB), m_appliedImpulse(btScalar(0.)), -m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE) +m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE), +m_jointFeedback(0) { } diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h index a16e869a973..441fa375050 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h @@ -16,9 +16,10 @@ subject to the following restrictions: #ifndef BT_TYPED_CONSTRAINT_H #define BT_TYPED_CONSTRAINT_H -class btRigidBody; + #include "LinearMath/btScalar.h" #include "btSolverConstraint.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" class btSerializer; @@ -32,6 +33,7 @@ enum btTypedConstraintType SLIDER_CONSTRAINT_TYPE, CONTACT_CONSTRAINT_TYPE, D6_SPRING_CONSTRAINT_TYPE, + GEAR_CONSTRAINT_TYPE, MAX_CONSTRAINT_TYPE }; @@ -51,8 +53,17 @@ enum btConstraintParams #endif +ATTRIBUTE_ALIGNED16(struct) btJointFeedback +{ + btVector3 m_appliedForceBodyA; + btVector3 m_appliedTorqueBodyA; + btVector3 m_appliedForceBodyB; + btVector3 m_appliedTorqueBodyB; +}; + + ///TypedConstraint is the baseclass for Bullet constraints and vehicles -class btTypedConstraint : public btTypedObject +ATTRIBUTE_ALIGNED16(class) btTypedConstraint : public btTypedObject { int m_userConstraintType; @@ -80,6 +91,7 @@ protected: btRigidBody& m_rbB; btScalar m_appliedImpulse; btScalar m_dbgDrawSize; + btJointFeedback* m_jointFeedback; ///internal method used by the constraint solver, don't use them directly btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact); @@ -87,6 +99,8 @@ protected: public: + BT_DECLARE_ALIGNED_ALLOCATOR(); + virtual ~btTypedConstraint() {}; btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA); btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB); @@ -195,7 +209,7 @@ public: ///internal method used by the constraint solver, don't use them directly - virtual void solveConstraintObsolete(btRigidBody& /*bodyA*/,btRigidBody& /*bodyB*/,btScalar /*timeStep*/) {}; + virtual void solveConstraintObsolete(btSolverBody& /*bodyA*/,btSolverBody& /*bodyB*/,btScalar /*timeStep*/) {}; const btRigidBody& getRigidBodyA() const @@ -246,6 +260,22 @@ public: return m_userConstraintPtr; } + void setJointFeedback(btJointFeedback* jointFeedback) + { + m_jointFeedback = jointFeedback; + } + + const btJointFeedback* getJointFeedback() const + { + return m_jointFeedback; + } + + btJointFeedback* getJointFeedback() + { + return m_jointFeedback; + } + + int getUid() const { return m_userConstraintId; diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h index a86939164ec..9e708410430 100644 --- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h +++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btUniversalConstraint.h @@ -31,13 +31,16 @@ subject to the following restrictions: /// "Given axis 1 on body 1, and axis 2 on body 2 that is perpendicular to axis 1, it keeps them perpendicular. /// In other words, rotation of the two bodies about the direction perpendicular to the two axes will be equal." -class btUniversalConstraint : public btGeneric6DofConstraint +ATTRIBUTE_ALIGNED16(class) btUniversalConstraint : public btGeneric6DofConstraint { protected: btVector3 m_anchor; btVector3 m_axis1; btVector3 m_axis2; public: + + BT_DECLARE_ALIGNED_ALLOCATOR(); + // constructor // anchor, axis1 and axis2 are in world coordinate system // axis1 must be orthogonal to axis2 -- cgit v1.2.3